Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 05:45:59 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-08-01 15:21:47 +00:00
parent 34b2263546
commit 6bff32f6c4
5 changed files with 404 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/barometer/baro_ekf.sce
+104
View File
@@ -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");
sw/in_progress/barometer/baro_ekf2.sce
+108
View File
@@ -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");
sw/in_progress/barometer/baro_utils.sci
+88 -1
View File
@@ -19,4 +19,91 @@ while meof(u) == 0,
end
mclose(u);
endfunction
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
sw/in_progress/barometer/ekf.sci
+70
View File
@@ -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
sw/in_progress/barometer/test1.sce
+34 -1
View File
@@ -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);
//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");