|
|
|
@ -766,7 +766,7 @@ void NavEKF3_core::fuseEulerYaw()
@@ -766,7 +766,7 @@ void NavEKF3_core::fuseEulerYaw()
|
|
|
|
|
Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
// calculate observaton jacobian when we are observing a rotation in a 312 sequence
|
|
|
|
|
// calculate observation jacobian when we are observing a rotation in a 312 sequence
|
|
|
|
|
float t9 = q0*q3; |
|
|
|
|
float t10 = q1*q2; |
|
|
|
|
float t2 = t9-t10; |
|
|
|
@ -808,7 +808,7 @@ void NavEKF3_core::fuseEulerYaw()
@@ -808,7 +808,7 @@ void NavEKF3_core::fuseEulerYaw()
|
|
|
|
|
Vector3f magMeasNED = Tbn_zeroYaw*magDataDelayed.mag; |
|
|
|
|
|
|
|
|
|
// Use the difference between the horizontal projection and declination to give the measured yaw
|
|
|
|
|
// If we can't use compass data, set the meaurement to the predicted
|
|
|
|
|
// If we can't use compass data, set the measurement to the predicted
|
|
|
|
|
// to prevent uncontrolled variance growth whilst on ground without magnetometer
|
|
|
|
|
float measured_yaw; |
|
|
|
|
if (use_compass() && yawAlignComplete) { |
|
|
|
@ -1119,7 +1119,7 @@ void NavEKF3_core::alignMagStateDeclination()
@@ -1119,7 +1119,7 @@ void NavEKF3_core::alignMagStateDeclination()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// record a magentic field state reset event
|
|
|
|
|
// record a magnetic field state reset event
|
|
|
|
|
void NavEKF3_core::recordMagReset() |
|
|
|
|
{ |
|
|
|
|
magStateInitComplete = true; |
|
|
|
|