diff --git a/sw/in_progress/barometer/baro_ekf.sce b/sw/in_progress/barometer/baro_ekf.sce new file mode 100644 index 0000000000..41a42238a1 --- /dev/null +++ b/sw/in_progress/barometer/baro_ekf.sce @@ -0,0 +1,104 @@ +// +// +// +// + + +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); + +predict_only = 1; +dt = (time(length(time)) - time(1)) / length(time) + +// +// Initialisation +// +[alt, a, b] = filter_init(10, pressure, gps_alt) + +X0 = [ alt; a; b ]; + +P0 = [ 1. 0. 0. + 0. 1. 0. + 0. 0. 1. ]; + +X = [X0]; +P = [P0]; + +// +// 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 ]; + + [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); + + // 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"); +subplot(4,1,2) +xtitle('a'); +plot2d([time]', [X(2,:)]', style=[5], leg="a"); +subplot(4,1,3) +xtitle('b'); +plot2d([time]', [X(3,:)]', style=[5], leg="b"); +subplot(4,1,4) +xtitle('covariance'); +P11 = []; +P22 = []; +P33 = []; +for i=1:length(time) + 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 diff --git a/sw/in_progress/barometer/baro_ekf2.sce b/sw/in_progress/barometer/baro_ekf2.sce new file mode 100644 index 0000000000..fe7cab53ec --- /dev/null +++ b/sw/in_progress/barometer/baro_ekf2.sce @@ -0,0 +1,108 @@ +// +// +// +// + + +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); + +predict_only = 1; +dt = (time(length(time)) - time(1)) / length(time) + +// +// Initialisation +// +[alt, a, b] = filter_init(10, pressure, gps_alt) + +X0 = [ (alt-b)/a; a; b ]; + +P0 = [ 1. 0. 0. + 0. 1. 0. + 0. 0. 1. ]; + +X = [X0]; +P = [P0]; + +// +// 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; + // time derivative of state + X0dot = [ pdot; 0; 0 ]; + // Jacobian of Xdot wrt X + F = [ 0 0 0 + 0 0 0 + 0 0 0 ]; + // process covariance noise + Q = [10. 0. 0. + 0. .1 0. + 0. 0. 0.1 ]; + + [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) * Xpred(2) - Xpred(3); + // Jacobian of measurement wrt X + H = [ 1 0 0]; + // Measurement covariance noise + R = 10.; + [Xup, Pup] = ekf_update(Xpred, Ppred, H, R, err); + + // measurement pressure + measure = pressure(i); +// err + + X = [X Xup]; + P = [P Pup]; + + end +end + + + +// +// Display +// +xbasc(); +subplot(4,1,1) +xtitle('altitude'); + +est_alt = []; +for i=1:length(time) + est_alt = [est_alt X(1,i) * X(2,i) + X(3,i)]; +end +plot2d([time; time]', [gps_alt; est_alt]', style=[5, 3], leg="gps@estimate"); +subplot(4,1,2) +xtitle('a'); +plot2d([time]', [X(2,:)]', style=[5], leg="a"); +subplot(4,1,3) +xtitle('b'); +plot2d([time]', [X(3,:)]', style=[5], leg="b"); +subplot(4,1,4) +xtitle('covariance'); +P11 = []; +P22 = []; +P33 = []; +for i=1:length(time) + 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="p@a@b"); \ 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 b2c772b1ce..ea8cfcfcfd 100644 --- a/sw/in_progress/barometer/baro_utils.sci +++ b/sw/in_progress/barometer/baro_utils.sci @@ -19,4 +19,91 @@ while meof(u) == 0, end mclose(u); -endfunction \ No newline at end of file +endfunction + + + +function [baro_alt] = compute_altitude_full(baro_pressure) +P0 = 1013.25; +T0 = 288.15; +Tg =6.5/1000; +R = 287.052; +g = 9.81; +baro_alt=[]; +len = length(baro_pressure) +for i=1:len + ba = T0/Tg*(1-(baro_pressure(i)/P0)^(Tg*R/g)); + baro_alt=[baro_alt ba]; +end +endfunction + +function [baro_alt] = compute_altitude_lin(baro_pressure) +baro_alt=[]; +len = length(baro_pressure) +for idx=1:len + p = 10 * baro_pressure(idx); + if (p > 10300) + i = 1638.; + j = -139.; + pl = 10300.; + elseif ( p > 9700 ) + i = 1720.; + j = 365.; + pl = 9700.; + elseif ( p > 9200 ) + i = 1802.; + j = 805.; + pl = 9200.; + elseif ( p > 8500 ) + i = 1905.; + j = 1456.; + pl = 8500.; + elseif ( p > 7800 ) + + elseif ( p > 7100 ) + + end + ba = j - (p-pl) * i / 2^11; + baro_alt = [baro_alt ba]; +end + +endfunction + + +// +// intersema application note 501 page 8 +// +function [alt, a, b] = filter_init(avg_len, pressure, gps_alt) + + avg_pressure = sum(pressure(1:avg_len), 'c') / avg_len; + avg_gps = sum(gps_alt(1:avg_len), 'c') / avg_len; + + if (avg_pressure > 1030.0) + a = -16380. / 2^11; + elseif (avg_pressure > 970.0) + a = -17200. / 2^11; + elseif (avg_pressure > 920.0) + a = -18020. / 2^11; + elseif (avg_pressure > 850.0) + a = -19050. / 2^11; + elseif (avg_pressure > 780.0) + a = -20330. / 2^11; + elseif (avg_pressure > 710.0) + a = -21880. / 2^11; + elseif (avg_pressure > 650.0) + a = -23590. / 2^11; + end; + + b = avg_gps - a * avg_pressure; + alt = avg_gps; + +endfunction + + + +function [Pi] = baro_get_P(P, i) + + Pi = P(:, 1+3*(i-1):3*i); + +endfunction + diff --git a/sw/in_progress/barometer/ekf.sci b/sw/in_progress/barometer/ekf.sci new file mode 100644 index 0000000000..6e66e093f4 --- /dev/null +++ b/sw/in_progress/barometer/ekf.sci @@ -0,0 +1,70 @@ + +// +// +// +// +// +// +// +function [X1, P1] = ekf_predict_continuous(X0, X0dot, dt, P0, F, Q) + +X1 = X0 + X0dot * dt; +P0dot = F*P0 + P0*F' + Q; +P1 = P0 + P0dot * dt; + +endfunction + +// +// +// +// +// +// +// +function [X1, P1] = ekf_predict_discrete(X0, X0dot, dt, P0, F, Q) + +X1 = X0 + X0dot * dt; +expF = expm(dt * F); +P0dot = expF*P0*expF' + Q; +P1 = P0 + P0dot * dt; + +endfunction + +// +// +// +// +// +// +// +function [X1, P1] = ekf_update(X0, P0, H, R, err) + +E = H * P0 * H' + R; +K = P0 * H' * inv(E); +P1 = P0 - K * H * P0; +X1 = X0 + K * err; + +endfunction + + + +// +// Pade approximation of matrix exponential +// +function [expA] = mat_exp(A, epsilon) + + normA = norm(A, 'inf'); + //Ns = max(0, int(normA)) ??? + Ns = normA; + ki = 2^(-Ns) * A; + + i=1; +// foo = 2^(3-2*i)* + + +expA = A; + + +endfunction + + diff --git a/sw/in_progress/barometer/test1.sce b/sw/in_progress/barometer/test1.sce index 2464343b15..ed46a06da9 100644 --- a/sw/in_progress/barometer/test1.sce +++ b/sw/in_progress/barometer/test1.sce @@ -1,7 +1,40 @@ + + + clear(); getf('baro_utils.sci'); filename = "data/07_07_26__16_37_09.baro.txt"; [time, gps_alt, pressure, gps_climb, temp] = baro_read_log(filename); -plot2d(time, gps_alt); \ No newline at end of file +//a = -17000/2^11; +//b = 1000. * 17000. / 2^11; + +[a, b] = filter_init(10, pressure, gps_alt) + +if 0 + deff('e=G(p,z)','a=p(1),b=p(2), gps=z(1), pressure=z(2), e=gps-(a/pressure+b)') + deff('dg=dG(p,z)','a=p(1),b=p(2), gps=z(1), pressure=z(2), dg = [-1/pressure; -b]') + p0 = [a, 0]'; + [p, err] = fit_dat(G, p0, [gps_alt; pressure], [1], dG) + + a = p(1) + b = p(2) +end + +baro_alt_lin = a * pressure + b; +baro_alt_full = compute_altitude_full(pressure); +//baro_alt = compute_altitude_lin(pressure); + +xbasc(); +if 0 + subplot(3,1,1) + xtitle('Gps'); + plot2d([time], [gps_alt]); + subplot(3,1,2) + xtitle('Pressure'); + plot2d([time], [pressure]); + subplot(3,1,3) +end +xtitle('Both'); +plot2d([time; time; time]', [gps_alt; baro_alt_full; baro_alt_lin]', style=[5, 3, 2], leg="gps@barofull@barolin"); \ No newline at end of file