Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-22 04:13:39 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-07-06 12:58:13 +00:00
parent eb205d2aa6
commit 63bc235266
6 changed files with 120 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/inertial/scilab/README
+3
View File
@@ -12,3 +12,6 @@ ahrs_euler_ekf_seqm:
2/ ahrs_quat
ahrs_quat_ekf_compm:
quaternion + bias estimation using ekf filter and complete measurements ( all 3 euler angles for every update)
sw/in_progress/inertial/scilab/ahrs_euler_ekf_compm.sce
+5 -5
View File
@@ -45,16 +45,16 @@ Q = [ Qe 0 0 0 0 0
0 0 0 0 Qb 0
0 0 0 0 0 Qb ];
// measure covariance noise
// measurement covariance noise
R = [
1.3^2 0. 0.
0. 1.3^2 0.
0. 0. 2.5^2
];
X=[X0];
P=[P0];
M=[X0(1:3)];
X=[X0]; // state
P=[P0]; // state covariance
M=[X0(1:3)]; // measurements
for i=2:length(time)
// command
@@ -89,4 +89,4 @@ for i=2:length(time)
end
ahrsh_euler_display(time, X, P, M)
ahrs_euler_display(time, X, P, M)
sw/in_progress/inertial/scilab/ahrs_euler_ekf_seqm.sce
+1 -1
View File
@@ -113,4 +113,4 @@ for i=2:length(time)
end
ahrsh_euler_display(time, X, P, M)
ahrs_euler_display(time, X, P, M)
sw/in_progress/inertial/scilab/ahrs_euler_utils.sci
+1 -1
View File
@@ -39,7 +39,7 @@ endfunction
//
//
//
function [] = ahrsh_euler_display(time, X, P, M)
function [] = ahrs_euler_display(time, X, P, M)
xbasc();
subplot(3,1,1)
xtitle('Angle');
sw/in_progress/inertial/scilab/ahrs_quat_utils.sci
+107 -16
View File
@@ -1,8 +1,3 @@
//
//
//
@@ -12,21 +7,76 @@
//
function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro)
[AE0] = ahrs_euler_init(avg_len, accel, mag, gyro);
[AEX0] = ahrs_euler_init(avg_len, accel, mag, gyro);
[quat] = quat_of_euler(AE0(1:3));
[quat] = quat_of_euler(AEX0(1:3));
X0 = [ quat AE0(4:6) ]';
X0 = [ quat; AEX0(4:6) ];
endfunction
//
//
//
// Filter
//
//
//
function [Pi] = ahrs_quat_get_P(P, i)
Pi = P(:, 1+7*(i-1):7*i);
endfunction
//
//
//
// Time derivative of state
//
//
//
function [Xdot] = ahrs_quat_get_Xdot(X, rate)
p = rate(1);
q = rate(2);
r = rate(3);
OMEGA = 1/2 * [ 0 -p -q -r 0 0 0
p 0 r -q 0 0 0
q -r 0 p 0 0 0
r q -p 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0 ];
Xdot = OMEGA * X;
endfunction
//
//
//
// Derivative of Xdot wrt X
//
//
//
function [F] = ahrs_quat_get_F(X, rate)
q0 = X(1);
q1 = X(2);
q2 = X(3);
q3 = X(4);
p = rate(1);
q = rate(2);
r = rate(3);
F = 1/2 * [ 0 -p -q -r q1 q2 q3;
p 0 r -q -q0 q3 -q2;
q -r 0 p -q3 q0 q1;
r q -p 0 q2 -q1 -q0;
0 0 0 0 0 0 0;
0 0 0 0 0 0 0;
0 0 0 0 0 0 0 ];
endfunction
//
@@ -56,13 +106,6 @@ H = [
endfunction
function [H] = ahrs_quat_get_dtheta_dX(X)
DCM = dcm_of_quat(X(1:4));
@@ -101,4 +144,52 @@ H = [
0
]';
endfunction
endfunction
function [H] = ahrs_quat_compm_get_deuler_dX(X)
Hphi = ahrs_quat_get_dphi_dX(X);
Htheta = ahrs_quat_get_dtheta_dX(X);
Hpsi = ahrs_quat_get_dpsi_dX(X);
H = [ Hphi; Htheta; Hpsi];
endfunction
//
//
//
// Display
//
//
//
function [] = ahrs_quat_display(time, X, P, M)
xbasc();
subplot(3,1,1)
xtitle('Angle');
EstEulerDeg = [];
for i=1:length(time)
ead = euler_of_quat(X(1:4, i)) * 180 / %pi;
EstEulerDeg = [EstEulerDeg ead];
end
MeasAngleDeg = M * 180 / %pi;
plot2d([time; time; time]', EstEulerDeg', style=[5, 3, 2], leg="phi@theta@psi");
plot2d([time; time; time]', MeasAngleDeg', style=[0, 0, 0]);
subplot(3,1,2)
xtitle('Bias');
EstBiasDeg = X(5:7,:) * 180 / %pi;
plot2d([time; time; time]', EstBiasDeg', style=[5, 3, 2], leg="phi@theta@psi");
subplot(3,1,3)
xtitle('Covariance');
Pdiag = [];
for i=1:length(time)
Pi = ahrs_quat_get_P(P, i);
Pdiag = [Pdiag [Pi(1,1) Pi(2,2) Pi(3,3) Pi(4,4) Pi(5,5) Pi(6,6) Pi(7,7)]'];
end
plot2d([time; time; time; time; time; time; time]', Pdiag', style=[5, 4, 3, 2, 0, 0, 0],...
leg="q0@q1@q2@q3@bp@bq@br");
endfunction
sw/in_progress/inertial/scilab/rotations.sci
+3 -3
View File
@@ -22,7 +22,7 @@ q1 = -cosphi2 * sintheta2 * sinpsi2 + sinphi2 * costheta2 * cospsi2;
q2 = cosphi2 * sintheta2 * cospsi2 + sinphi2 * costheta2 * sinpsi2;
q3 = cosphi2 * costheta2 * sinpsi2 - sinphi2 * sintheta2 * cospsi2;
quat = [q0 q1 q2 q3];
quat = [q0 q1 q2 q3]';
endfunction
@@ -38,13 +38,13 @@ function [euler] = euler_of_quat(quat)
theta = -asin( dcm02 );
psi = atan( dcm01, dcm00 );
euler = [phi theta psi];
euler = [phi; theta; psi];
endfunction
function [X1] = normalise_quat(X0)
quat = X0(1:4);
quat = quat / norm(quat);
X1 = [quat; X0(5:7)];
X1 = [quat];
endfunction