From f5e9ea16268c35411dac093560ebe07aba046948 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Jun 2016 17:03:44 +1000 Subject: [PATCH] EKF: Prevent potential divide by zero --- EKF/ekf_helper.cpp | 36 +++++++++++++++++++++--------------- 1 file changed, 21 insertions(+), 15 deletions(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 4734ba2a3d..666e60f54e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -815,21 +815,27 @@ Vector3f Ekf::calcRotVecVariances() float t3 = acos(q0); float t4 = -t2+1.0f; float t5 = t2-1.0f; - float t6 = 1.0f/t5; - float t7 = q1*t6*2.0f; - float t8 = 1.0f/powf(t4,1.5f); - float t9 = q0*q1*t3*t8*2.0f; - float t10 = t7+t9; - float t11 = 1.0f/sqrtf(t4); - float t12 = q2*t6*2.0f; - float t13 = q0*q2*t3*t8*2.0f; - float t14 = t12+t13; - float t15 = q3*t6*2.0f; - float t16 = q0*q3*t3*t8*2.0f; - float t17 = t15+t16; - rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; - rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; - rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + if ((t4 > 1e-9f) && (t5 < -1e-9f)) { + float t6 = 1.0f/t5; + float t7 = q1*t6*2.0f; + float t8 = 1.0f/powf(t4,1.5f); + float t9 = q0*q1*t3*t8*2.0f; + float t10 = t7+t9; + float t11 = 1.0f/sqrtf(t4); + float t12 = q2*t6*2.0f; + float t13 = q0*q2*t3*t8*2.0f; + float t14 = t12+t13; + float t15 = q3*t6*2.0f; + float t16 = q0*q3*t3*t8*2.0f; + float t17 = t15+t16; + rot_var_vec(0) = t10*(P[0][0]*t10+P[1][0]*t3*t11*2.0f)+t3*t11*(P[0][1]*t10+P[1][1]*t3*t11*2.0f)*2.0f; + rot_var_vec(1) = t14*(P[0][0]*t14+P[2][0]*t3*t11*2.0f)+t3*t11*(P[0][2]*t14+P[2][2]*t3*t11*2.0f)*2.0f; + rot_var_vec(2) = t17*(P[0][0]*t17+P[3][0]*t3*t11*2.0f)+t3*t11*(P[0][3]*t17+P[3][3]*t3*t11*2.0f)*2.0f; + } else { + rot_var_vec(0) = 4.0f * P[1][1]; + rot_var_vec(1) = 4.0f * P[2][2]; + rot_var_vec(2) = 4.0f * P[3][3]; + } return rot_var_vec; }