Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-20 02:53:15 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-07-04 14:56:46 +00:00
parent 11256ac081
commit eb205d2aa6
3 changed files with 109 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/in_progress/inertial/scilab/README
+4 -1
View File
@@ -1,4 +1,4 @@
tilt :
0/ tilt
1/ ahrs_euler
@@ -9,3 +9,6 @@ ahrs_euler_ekf_compm:
ahrs_euler_ekf_seqm:
euler angles + bias estimation using ekf filter and sequential measurements ( one of each euler angle every update)
2/ ahrs_quat
sw/in_progress/inertial/scilab/ahrs_quat_ekf_seqm.sce
+1 -1
View File
@@ -1,7 +1,7 @@
clear();
getf('rotations.sci');
getf('imu.sci');
getf('ahrs_utils.sci');
getf('ahrs_quat_utils.sci');
getf('ekf.sci');
use_sim = 1;
sw/in_progress/inertial/scilab/ahrs_quat_utils.sci
+104
View File
@@ -0,0 +1,104 @@
//
//
//
// Initialisation
//
//
//
function [X0] = ahrs_quat_init(avg_len, accel, mag, gyro)
[AE0] = ahrs_euler_init(avg_len, accel, mag, gyro);
[quat] = quat_of_euler(AE0(1:3));
X0 = [ quat AE0(4:6) ]';
endfunction
//
//
//
// Derivative of measure wrt state
//
//
//
function [H] = ahrs_quat_get_dphi_dX(X)
DCM = dcm_of_quat(X(1:4));
phi_err = 2 / (DCM(3,3)^2 + DCM(2,3)^2);
q0 = X(1);
q1 = X(2);
q2 = X(3);
q3 = X(4);
H = [
(q1 * DCM(3,3)) * phi_err
(q0 * DCM(3,3) + 2 * q1 * DCM(2,3)) * phi_err
(q3 * DCM(3,3) + 2 * q2 * DCM(2,3)) * phi_err
(q2 * DCM(3,3)) * phi_err
0
0
0
]';
endfunction
function [H] = ahrs_quat_get_dtheta_dX(X)
DCM = dcm_of_quat(X(1:4));
theta_err = 2 / sqrt(1 - DCM(1,3)^2);
q0 = X(1);
q1 = X(2);
q2 = X(3);
q3 = X(4);
H = [
q2 * theta_err
-q3 * theta_err
q0 * theta_err
-q1 * theta_err
0
0
0
]';
endfunction
function [H] = ahrs_quat_get_dpsi_dX(X)
DCM = dcm_of_quat(X(1:4));
psi_err = 2 / (DCM(1,1)^2 + DCM(1,2)^2);
q0 = X(1);
q1 = X(2);
q2 = X(3);
q3 = X(4);
H = [
(q3 * DCM(1,1)) * psi_err
(q2 * DCM(1,1)) * psi_err
(q1 * DCM(1,1) + 2 * q2 * DCM(1,2)) * psi_err
(q0 * DCM(1,1) + 2 * q3 * DCM(1,2)) * psi_err
0
0
0
]';
endfunction