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

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-08-02 23:35:59 +00:00
parent 16cc2fd725
commit 2667c915c3
1 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/barometer/baro_ekf.sce
+11 -6
View File
@@ -49,14 +49,14 @@ for idx_p=(end_pressure+1):length(time_pressure)
F = [ 1. delta_p F = [ 1. delta_p
0 1. ]; 0 1. ];
// process covariance noise // process covariance noise
Q = [ 1e-5 0. Q = [ 1e-4 0.
0. 1e-7 ]; 0. 1e-6 ];
P1 = F*P0*F' + Q; P1 = F*P0*F' + Q;
// update // update
err = altitude(idx_a) - X1(1); err = altitude(idx_a) - X1(1);
H = [1 0]; H = [1 0];
R = 10; R = 25;
E = H * P1 * H' + R; E = H * P1 * H' + R;
K = P1 * H' * inv(E); K = P1 * H' * inv(E);
P2 = P1 - K * H * P1; P2 = P1 - K * H * P1;
@@ -74,20 +74,25 @@ for idx_p=(end_pressure+1):length(time_pressure)
end end
dumb_alt = a0 * pressure + b0;
// //
// Display // Display
// //
xbasc(); xbasc();
subplot(4,1,1) subplot(4,1,1)
xtitle('altitude'); xtitle('altitude');
plot2d([time_altitude]', [altitude]', style=[5], leg="gps"); plot2d([time_altitude]', [altitude]', style=[5]);
plot2d([time_state]', [X(1,:)]', style=[3], leg="est_alt"); plot2d([time_state]', [X(1,:)]', style=[3, 5], leg="est_alt@gps");
plot2d([time_pressure]', [dumb_alt]', style=[1]);
subplot(4,1,2) subplot(4,1,2)
xtitle('pressure'); xtitle('pressure');
plot2d([time_pressure]', [pressure]', style=[5], leg="pressure"); plot2d([time_pressure]', [pressure]', style=[5], leg="pressure");
subplot(4,1,3) subplot(4,1,3)
xtitle('a'); xtitle('a');
plot2d([time_state]', [X(2,:)]', style=[5], leg="a"); plot2d([time_state]', [X(2,:)]', style=[3], leg="a");
subplot(4,1,4) subplot(4,1,4)
xtitle('covariance'); xtitle('covariance');
P11 = []; P11 = [];