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

EKF: Update cleaned up autocode fragment with sign error fix and missing LD

Browse Source
This commit is contained in:
Paul Riseborough
2019-01-28 11:56:39 +11:00
committed by Daniel Agar
parent fe378fd761
commit 7bddbd1cc5
1 changed files with 19 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/matlab/scripts/Inertial Nav EKF/YawCovariance.c
+19 -11
View File
@@ -3,6 +3,8 @@
The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions
The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
See DeriveYawResetEquations.m for the derivation
The gnerate autocode has been cleaned up with removal of 0 coefficient terms and mirroring of lower
diagonal terms missing from the derivation script raw autocode output of C_code4.txt
*/
// Intermediate variables
@@ -20,15 +22,21 @@ SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2])
// Variance of yaw angle uncertainty (rad**2)
const float daYawVar = TBD;
// Add covariances for additonal yaw uncertainty to exisiting covariances.
// Add covariances for additonal yaw uncertainty to existing covariances.
// This assumes that the additional yaw error is uncorrrelated
P[0][0] += daYawVar*sq(SQ[2]);
P[0][1] += daYawVar*SQ[1]*SQ[2];
P[1][1] += daYawVar*sq(SQ[1]);
P[0][2] += daYawVar*SQ[0]*SQ[2];
P[1][2] += daYawVar*SQ[0]*SQ[1];
P[2][2] += daYawVar*sq(SQ[0]);
P[0][3] += daYawVar*SQ[2]*SQ[3];
P[1][3] += daYawVar*SQ[1]*SQ[3];
P[2][3] += daYawVar*SQ[0]*SQ[3];
P[3][3] += daYawVar*sq(SQ[3]);
P[0][0] += yaw_variance*sq(SQ[2]);
P[0][1] += yaw_variance*SQ[1]*SQ[2];
P[1][1] += yaw_variance*sq(SQ[1]);
P[0][2] += yaw_variance*SQ[0]*SQ[2];
P[1][2] += yaw_variance*SQ[0]*SQ[1];
P[2][2] += yaw_variance*sq(SQ[0]);
P[0][3] -= yaw_variance*SQ[2]*SQ[3];
P[1][3] -= yaw_variance*SQ[1]*SQ[3];
P[2][3] -= yaw_variance*SQ[0]*SQ[3];
P[3][3] += yaw_variance*sq(SQ[3]);
P[1][0] += yaw_variance*SQ[1]*SQ[2];
P[2][0] += yaw_variance*SQ[0]*SQ[2];
P[2][1] += yaw_variance*SQ[0]*SQ[1];
P[3][0] -= yaw_variance*SQ[2]*SQ[3];
P[3][1] -= yaw_variance*SQ[1]*SQ[3];
P[3][2] -= yaw_variance*SQ[0]*SQ[3];