AP_NavEKF3: Fix error in scaling of observation error

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -958,7 +958,7 @@ void NavEKF3_core::FuseBodyVel()
             }
 
             // calculate intermediate expressions for X axis Kalman gains
-            float R_VEL = bodyOdmDataDelayed.velErr;
+            float R_VEL = sq(bodyOdmDataDelayed.velErr);
             float t2 = q0*q3*2.0f;
             float t3 = q1*q2*2.0f;
             float t4 = t2+t3;
@@ -1130,7 +1130,7 @@ void NavEKF3_core::FuseBodyVel()
             }
 
             // calculate intermediate expressions for Y axis Kalman gains
-            float R_VEL = bodyOdmDataDelayed.velErr;
+            float R_VEL = sq(bodyOdmDataDelayed.velErr);
             float t2 = q0*q3*2.0f;
             float t9 = q1*q2*2.0f;
             float t3 = t2-t9;
@@ -1302,7 +1302,7 @@ void NavEKF3_core::FuseBodyVel()
             }
 
             // calculate intermediate expressions for Z axis Kalman gains
-            float R_VEL = bodyOdmDataDelayed.velErr;
+            float R_VEL = sq(bodyOdmDataDelayed.velErr);
             float t2 = q0*q2*2.0f;
             float t3 = q1*q3*2.0f;
             float t4 = t2+t3;