From: Keith Packard Date: Fri, 26 Jun 2009 07:18:24 +0000 (-0700) Subject: Fix up data conversion to use "real" LP filters X-Git-Url: https://git.gag.com/?a=commitdiff_plain;h=afa1f2dcc7affe9973f7940b6573a99ce27eeab5;p=fw%2Ftmflights Fix up data conversion to use "real" LP filters --- diff --git a/filter.5c b/filter.5c new file mode 100644 index 0000000..34609ee --- /dev/null +++ b/filter.5c @@ -0,0 +1,133 @@ +/* + * Kaiser Window digital filter + * + * Copyright © 2001, Keith Packard + * All Rights Reserved. See the file COPYING in this directory + * for licensing information. + */ + +real i0(real x) +{ + real ds, d, s; + + ds = 1; + d = 0; + s = 0; + do + { + d = d + 2; + ds = ds * x**2 / d**2; + s = s + ds; +# printf ("ds %g s %g dist %g\n", ds, s, +# ds - 0.2e-8 * s); + } + while (ds - 0.2e-8 * s > 0); + return s; +} + +real highpass (real n, real m, real wc) +{ + real alpha = m/2; + real dist; + + dist = n - alpha; + if (dist == 0) + return (pi/2 - wc) / pi; + return -sin(dist * (pi/2-wc)) / (pi * dist); +} + +real lowpass (real n, real m, real wc) +{ + real alpha = m/2; + real dist; + dist = n - alpha; + if (dist == 0) + return wc / pi; + return sin (wc * dist) / (pi * dist); +} + +real kaiser (real n, real m, real beta) +{ + real alpha = m / 2; + return i0 (beta * sqrt (1 - ((n - alpha) / alpha)**2)) / i0(beta); +} + +void write_high (string filename, + real m, + real wc, + real beta, + real step) +{ + real n; + file f; + + f = File::open (filename, "w"); + for (n = 0; n <= m; n += step) + File::fprintf (f, "%g,\n", highpass (n, m, wc) * kaiser (n, m, beta)); + File::close (f); +} + +void write_low (string filename, + real m, + real wc, + real beta, + real step) +{ + real n; + file f; + + f = File::open (filename, "w"); + for (n = 0; n <= m; n += step) + File::fprintf (f, "%g,\n", lowpass (n, m, wc) * kaiser (n, m, beta)); + File::close (f); +} + +real β (real A) +{ + if (A > 50) + return 0.1102 * (A - 8.7); + else if (A >= 21) + return 0.5842 * (A - 21) ** 0.4 + 0.07886 * (A - 21); + else + return 0.0; +} + +int M (real A, real Δω) +{ + if (A > 21) + return ceil ((A - 7.95) / (2.285 * Δω)); + else + return ceil(5.79 / Δω); +} + +typedef struct { + real ωpass; + real Δω; + real A; + real β; + int M; +} filter_param_t; + +filter_param_t filter (real ωpass, real ωstop, real error) +{ + filter_param_t p; + p.ωpass = ωpass; + p.Δω = ωstop - ωpass; + p.A = -20 * log10 (error); + p.β = β (p.A); + p.M = M (p.A, p.Δω); + if ((p.M & 1) == 1) + p.M++; + return p; +} + +real[] low_pass_filter(real ωpass, real ωstop, real error) +{ + filter_param_t p = filter(ωpass, ωstop, error); + + return (real[p.M + 1]) { + [n] = lowpass(n, p.M, ωpass) * kaiser(n, p.M, p.β) + }; +} + + diff --git a/kaiser-test.5c b/kaiser-test.5c new file mode 100644 index 0000000..41fd5e5 --- /dev/null +++ b/kaiser-test.5c @@ -0,0 +1,58 @@ +load "kaiser.5c" + +real sinc(real x) = x != 0 ? sin(x)/x : 1; + +real sum(real[...] x) { real s = 0; for(int i = 0; i < dim(x); i++) s += x[i]; return s; } + +real[...] convolve(real[...] d, real[...] e) { + real sample(n) = n < 0 ? d[0] : n >= dim(d) ? d[dim(d)-1] : d[n]; + real w = floor ((dim(e) - 1) / 2); + real c(int center) { + real v = 0; + real s = 0; + int start; + int end; + + start = center - w; + if (start < 0) + start = 0; + end = center + w; + if (end >= dim(d)) + end = dim(d) - 1; + for (int o = start; o <= end; o++) { + real e_o = e[o - center + w]; + real part = d[o] * e_o; + v += part; + s += e_o; + } + v /= s; + return v; + } + return (real[dim(d)]) { [n] = c(n) }; +} + +real[...] kaiser_filter(real[...] d, int half_width) { +# real[half_width * 2 + 1] fir = { [n] = sinc(2 * pi * n / (2 * half_width)) }; + real M = half_width * 2 + 1; + real[M] fir = { [n] = kaiser(n+1, M+1, 8) }; + return convolve(d, fir); +} + + +real[1024] square = { [n] = 2 * ((n >> 8) & 1) - 1 }; + +real[] cut = low_pass_filter(π*1/128, π*1/64, 1e-10); + +real[dim(square)] filtered_square = convolve(square, cut); + +for (int i = 0; i < dim(filtered_square); i++) + printf ("%d %g %g\n", i, square[i], filtered_square[i]); + +exit(0); + +int size = 100; +real[size] raw = { [n] = n }; +real[size] filtered = kaiser_filter(raw, 8); + +for (int i = 0; i < size; i++) + printf ("%d %g %g\n", i, raw[i], filtered[i]); diff --git a/parse b/parse index 7593b38..eb782c6 100755 --- a/parse +++ b/parse @@ -225,7 +225,7 @@ real sinc(real x) = x != 0 ? sin(x)/x : 1; real gaussian(real x) = exp(-(x**2)/2) / sqrt(2 * pi); -load "/usr/share/nickle/examples/kaiser.5c" +load "filter.5c" real[...] convolve(real[...] d, real[...] e) { real sample(n) = n < 0 ? d[0] : n >= dim(d) ? d[dim(d)-1] : d[n]; @@ -314,6 +314,10 @@ telem_record read_telem(file in) { } void readsamples_telem(file in) { + telem_record[...] telem; + + setdim(telem, 0); + setdim(clock, 0); setdim(pressure_value, 0); setdim(accelerometer_value, 0); @@ -334,23 +338,32 @@ void readsamples_telem(file in) { accel_total += save[i].accel; g_base = accel_total // start; - for (int i = start; i < dim(save); i++) { - clock[dim(clock)] = save[i].time/100; - pressure_value[dim(pressure_value)] = save[i].pressure; - accelerometer_value[dim(accelerometer_value)] = save[i].accel; - } - + for (int i = start; i < dim(save); i++) + telem[dim(telem)] = save[i]; + while (!File::end(in)) { - telem_record t = read_telem(in); - int n = dim(clock); - real sample_time = t.time / 100 + clock_bias; - if (n > 0 && sample_time < clock[n-1]) { - clock_bias += 65536 / 100; - sample_time += 65536 / 100; + int n = dim(telem); + telem[n] = read_telem(in); + telem[n].time += clock_bias; + if (n > 0 && telem[n].time < telem[n-1].time) { + clock_bias += 65536; + telem[n].time += 65536; + } + } + int clock_start = telem[0].time; + int clock_end = telem[dim(telem)-1].time; + int samples = clock_end - clock_start; + + int j = 0; + for (int i = 0; i < samples; i++) { + clock[i] = i / 100; + pressure_value[i] = telem[j].pressure; + accelerometer_value[i] = telem[j].accel; + if (j < dim(telem)-1) { + int cur_time = clock_start + i; + if (cur_time - telem[j].time > telem[j+1].time - cur_time) + j++; } - clock[n] = sample_time; - pressure_value[dim(pressure_value)] = t.pressure; - accelerometer_value[dim(accelerometer_value)] = t.accel; } } @@ -381,6 +394,12 @@ int[...] rebase(int[...] d, int m, int a) = (int[dim(d)]) { [n] = d[n] * m + a } int size = dim(accelerometer_value); +real[...] do_low_pass(real[] data, real ωpass, real ωstop, real error) { + real[*] fir = low_pass_filter (ωpass, ωstop, error); + File::fprintf (stderr, "low pass filter is %d long\n", dim(fir)); + return convolve(data, fir); +} + if (false) { accelerometer_value = rebase(accelerometer_value, -1, g_base); int accel_i0_base = average(accelerometer_value, 30); @@ -406,8 +425,14 @@ if (false) { } else { real[size] accelerometer = { [n] = gravity * (count_to_g(accelerometer_value[n]) - 1.0) }; real[size] barometer = { [n] = pressure_to_altitude(count_to_kPa(pressure_value[n] / 16) * 1000) }; - real[size] filtered_accelerometer = kaiser_filter(accelerometer, 8); - real[size] filtered_barometer = kaiser_filter(barometer, 16); + real[size] filtered_accelerometer = do_low_pass(accelerometer, + 2 * π * 5/100, + 2 * π * 8/100, + 1e-8); + real[size] filtered_barometer = do_low_pass(barometer, + 2 * π * .5 / 100, + 2 * π * 1 / 100, + 1e-8); real[...] integrate(real[...] d) { real[dim(d)] ret;