From 16cc2fd7257c63dc0f9d121ebe0cb9cf7bb5fb60 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Thu, 2 Aug 2007 22:41:46 +0000 Subject: [PATCH] *** empty log message *** --- sw/in_progress/barometer/baro_ekf.sce | 118 ++++++++++++------------ sw/in_progress/barometer/baro_utils.sci | 63 +++++++++++-- 2 files changed, 114 insertions(+), 67 deletions(-) diff --git a/sw/in_progress/barometer/baro_ekf.sce b/sw/in_progress/barometer/baro_ekf.sce index 41a42238a1..9e75d16cc3 100644 --- a/sw/in_progress/barometer/baro_ekf.sce +++ b/sw/in_progress/barometer/baro_ekf.sce @@ -8,97 +8,93 @@ clear(); getf('baro_utils.sci'); getf('ekf.sci'); -filename = "data/07_07_26__16_37_09.baro.txt"; -[time, gps_alt, pressure, gps_climb, temp] = baro_read_log(filename); +filename = "data/07_08_02__14_57_30.data"; +[time_pressure, pressure, time_altitude, altitude] = baro_read_pprz_log(filename); + +//[time, gps_alt, pressure, gps_climb, temp] = baro_read_log(filename); -predict_only = 1; -dt = (time(length(time)) - time(1)) / length(time) // // Initialisation // -[alt, a, b] = filter_init(10, pressure, gps_alt) +time_start = 110.; +time_end = 250.; +[pressure0,end_pressure, altitude0, end_altitude, a0, b0] = filter_init_timed(time_start, time_end, time_pressure, pressure, time_altitude, altitude) -X0 = [ alt; a; b ]; - -P0 = [ 1. 0. 0. - 0. 1. 0. - 0. 0. 1. ]; +X0 = [ altitude0; a0]; +P0 = [ 1. 0. + 0. 2. ]; + X = [X0]; P = [P0]; - +time_state = [time_pressure(end_pressure)]; // // Iterations // -for i=2:length(gps_alt) - // initial state - X0 = X(:, i-1); - // initial covariance - P0 = baro_get_P(P, i-1); - // command - pdot = (pressure(i) - pressure(i-1))/dt; - climb = pdot * X0(2); - // time derivative of state - X0dot = [ climb; 0; 0]; - // Jacobian of Xdot wrt X - F = [ 0 pdot 0 - 0 0 0 - 0 0 0 ]; - // process covariance noise - Q = [10. 0. 0. - 0. .1 0. - 0. 0. 0.1 ]; +idx_a = end_altitude; +idx_s = 2; +for idx_p=(end_pressure+1):length(time_pressure) - [Xpred, Ppred] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q); - - if ( predict_only ) - X = [X Xpred]; - P = [P Ppred]; - else - // measurement GPS - measure = gps_alt(i); - err = measure - Xpred(1); - // Jacobian of measurement wrt X - H = [ 1 pressure(i) 1]; - // Measurement covariance noise - R = 10.; - [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); + // prediction + // initial state + X0 = X(:, idx_s-1); + // initial covariance + P0 = baro_get_P(P, idx_s-1); + // command + delta_p = pressure(idx_p) - pressure(idx_p-1); + delta_z = X0(2) * delta_p; + X1 = X0 + [delta_z; 0]; + // jacobian + F = [ 1. delta_p + 0 1. ]; + // process covariance noise + Q = [ 1e-5 0. + 0. 1e-7 ]; + P1 = F*P0*F' + Q; + + // update + err = altitude(idx_a) - X1(1); + H = [1 0]; + R = 10; + E = H * P1 * H' + R; + K = P1 * H' * inv(E); + P2 = P1 - K * H * P1; + X2 = X1 + K * err; + + X = [X X2]; + P = [P P2]; + time_state = [time_state time_pressure(idx_p)]; + idx_p = idx_p + 1; + idx_s = idx_s + 1; + while ((idx_p < length(time_pressure)) & ... + (time_altitude(idx_a) < time_pressure(idx_p)) & ... + (idx_a < length(time_altitude))), idx_a = idx_a + 1; end - // measurement pressure - measure = pressure(i); - err - - X = [X Xup]; - P = [P Pup]; - - end end - // // Display // xbasc(); subplot(4,1,1) xtitle('altitude'); -plot2d([time; time]', [gps_alt; X(1,:)]', style=[5, 3], leg="gps@estimate"); +plot2d([time_altitude]', [altitude]', style=[5], leg="gps"); +plot2d([time_state]', [X(1,:)]', style=[3], leg="est_alt"); subplot(4,1,2) -xtitle('a'); -plot2d([time]', [X(2,:)]', style=[5], leg="a"); +xtitle('pressure'); +plot2d([time_pressure]', [pressure]', style=[5], leg="pressure"); subplot(4,1,3) -xtitle('b'); -plot2d([time]', [X(3,:)]', style=[5], leg="b"); +xtitle('a'); +plot2d([time_state]', [X(2,:)]', style=[5], leg="a"); subplot(4,1,4) xtitle('covariance'); P11 = []; P22 = []; -P33 = []; -for i=1:length(time) +for i=1:length(time_state) Pi = baro_get_P(P, i); P11 = [P11 Pi(1,1)]; P22 = [P22 Pi(2,2)]; - P33 = [P33 Pi(3,3)]; end -plot2d([time; time; time]', [P11; P22; P33]', style=[5 3 2], leg="alt@a@b"); \ No newline at end of file +plot2d([time_state; time_state]', [P11; P22]', style=[5 3], leg="Palt@Pa"); \ No newline at end of file diff --git a/sw/in_progress/barometer/baro_utils.sci b/sw/in_progress/barometer/baro_utils.sci index 07b3bc9247..8650f5c96c 100644 --- a/sw/in_progress/barometer/baro_utils.sci +++ b/sw/in_progress/barometer/baro_utils.sci @@ -24,10 +24,12 @@ endfunction -function [time_pres, pres] = baro_read_pprz_log(filename) +function [time_pressure, pressure, time_gps, altitude] = baro_read_pprz_log(filename) -time_pres= []; -pres = []; +time_pressure= []; +pressure = []; +time_gps = []; +altitude = []; u=mopen(filename, 'r'); while meof(u) == 0, @@ -35,10 +37,16 @@ while meof(u) == 0, if (line == "") continue end [nb_scan, tip, ac, pr, te] = msscanf(1, line, '%f %d BARO_MS5534A %d %d'); if (nb_scan == 4) - time_pres = [time_pres tip]; - pres = [pres pr]; + time_pressure = [time_pressure tip]; + pressure = [pressure pr]; + else + [nb_scan, tig, ac, gmod, gue, gun, gcour, galt, gspeed, gclimb, gitow, guzone, gnerr] = ... + msscanf(1, line, '%f %d GPS %d %d %d %d %d %d %d %d %d %d'); + if (nb_scan == 12) + time_gps = [time_gps tig]; + altitude = [altitude galt]; + end end - end mclose(u); endfunction @@ -122,6 +130,49 @@ function [pres, alt, a, b] = filter_init(avg_len, pressure, gps_alt) endfunction +function [pressure0,end_pressure, altitude0, end_altitude, a0, b0] = filter_init_timed(time_start, time_end, time_pressure, pressure, time_altitude, altitude) + [avg_pressure, start_pressure, end_pressure] = average_period(time_start, time_end, time_pressure, pressure); + [avg_altitude, start_altitude, end_altitude] = average_period(time_start, time_end, time_altitude, altitude); + + if (avg_pressure > 1030.0) + a0 = -16380. / 2^11; + elseif (avg_pressure > 970.0) + a0 = -17200. / 2^11; + elseif (avg_pressure > 920.0) + a0 = -18020. / 2^11; + elseif (avg_pressure > 850.0) + a0 = -19050. / 2^11; + elseif (avg_pressure > 780.0) + a0 = -20330. / 2^11; + elseif (avg_pressure > 710.0) + a0 = -21880. / 2^11; + elseif (avg_pressure > 650.0) + a0 = -23590. / 2^11; + end; + + pressure0 = avg_pressure; + altitude0 = avg_altitude; + b0 = avg_altitude - a0 * avg_pressure; + +endfunction + + + + +function [average, idx_start, idx_end] = average_period(time_start, time_end, time_samples, samples) + idx_start = 1; + while ((time_samples(idx_start) < time_start) & (idx_start <= length(time_samples))), + idx_start = idx_start+1; + end + idx_end = idx_start; + while ((time_samples(idx_end) < time_end) & (idx_end <= length(time_samples))), + idx_end = idx_end+1; + end + average = sum(samples(idx_start:idx_end), 'c') / (idx_end - idx_start); +endfunction + + + function [Pi] = baro_get_P(P, i)