fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 07:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Updated derivation of Jacobians for direct heading measurement

Browse Source 
Adds an alternative using a 312 Euler sequence.
This commit is contained in:
Paul Riseborough
2016-03-01 15:21:36 +11:00
parent 2c9a814de1
commit ea29e39e5b
2 changed files with 42 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matlab/generated/Inertial Nav EKF/Yaw Angle Fusion.txt
+26 -8
View File
@@ -1,10 +1,8 @@
/*
Autocode for fusion of an Euler yaw measurement.
Protection against /0 and covariance matrix errors will need to be added.
Code fragments for fusion of an Euler yaw measurement from a 321 sequence.
*/
// intermediate variables
// calculate observation jacobians
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
@@ -19,9 +17,29 @@ float t12 = t10*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update
float H_MAG[3];
H_YAW[0] = 0.0f;
H_YAW[1] = t14*(t15*(q0*q1*2.0f-q2*q3*2.0f)+t9*t10*(q0*q2*2.0f+q1*q3*2.0f));
H_YAW[2] = t14*(t15*(t2-t3+t4-t5)+t9*t10*(t7-t8));
/*
Code fragments for fusion of an Euler yaw measurement from a 312 sequence.
*/
// calculate observation jacobian
float t2 = q0*q0;
float t3 = q1*q1;
float t4 = q2*q2;
float t5 = q3*q3;
float t6 = t2-t3+t4-t5;
float t7 = q0*q3*2.0f;
float t10 = q1*q2*2.0f;
float t8 = t7-t10;
float t9 = 1.0f/(t6*t6);
float t11 = t8*t8;
float t12 = t9*t11;
float t13 = t12+1.0f;
float t14 = 1.0f/t13;
float t15 = 1.0f/t6;
H_YAW[0] = -t14*(t15*(q0*q2*2.0f+q1*q3*2.0f)-t8*t9*(q0*q1*2.0f-q2*q3*2.0f));
H_YAW[1] = 0.0f;
H_YAW[2] = t14*(t15*(t2+t3-t4-t5)+t8*t9*(t7+t10));