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

EKF: Fix bug in quaternion covariance initialisation function

Browse Source 
The covariances between quaternion and non-quaternion states were not being updated
This commit is contained in:
Paul Riseborough
2016-06-08 13:23:30 +10:00
parent b3baab64f2
commit f26aca55a1
1 changed files with 5 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+5 -1
View File
@@ -921,7 +921,11 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var)
float t43 = t16-t25;
float t44 = t17-t36;
// auto-code generated using matlab symbolic toolbox
// zero all the quaternion covariances
zeroRows(P,0,3);
zeroCols(P,0,3);
// Update the quaternion internal covariances using auto-code generated using matlab symbolic toolbox
P[0][0] = rot_vec_var(0)*t2*t9*t10*0.25f+rot_vec_var(1)*t4*t9*t10*0.25f+rot_vec_var(2)*t5*t9*t10*0.25f;
P[0][1] = t22;
P[0][2] = t35+rotX*rot_vec_var(0)*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rot_vec_var(1)*t3*t11*t30*0.5f;