|
|
|
@ -689,8 +689,8 @@ void NavEKF2_core::FuseMagnetometer()
@@ -689,8 +689,8 @@ void NavEKF2_core::FuseMagnetometer()
|
|
|
|
|
MagTableConstrain(); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// the first 3 states represent the angular misalignment vector. This is
|
|
|
|
|
// is used to correct the estimated quaternion on the current time step
|
|
|
|
|
// the first 3 states represent the angular misalignment vector.
|
|
|
|
|
// This is used to correct the estimated quaternion on the current time step
|
|
|
|
|
stateStruct.quat.rotate(stateStruct.angErr); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
@ -964,8 +964,8 @@ void NavEKF2_core::fuseEulerYaw()
@@ -964,8 +964,8 @@ void NavEKF2_core::fuseEulerYaw()
|
|
|
|
|
statesArray[i] -= Kfusion[i] * innovation; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// the first 3 states represent the angular misalignment vector. This is
|
|
|
|
|
// is used to correct the estimated quaternion on the current time step
|
|
|
|
|
// the first 3 states represent the angular misalignment vector.
|
|
|
|
|
// This is used to correct the estimated quaternion on the current time step
|
|
|
|
|
stateStruct.quat.rotate(stateStruct.angErr); |
|
|
|
|
|
|
|
|
|
// record fusion event
|
|
|
|
@ -1095,8 +1095,8 @@ void NavEKF2_core::FuseDeclination(float declErr)
@@ -1095,8 +1095,8 @@ void NavEKF2_core::FuseDeclination(float declErr)
|
|
|
|
|
statesArray[j] = statesArray[j] - Kfusion[j] * innovation; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// the first 3 states represent the angular misalignment vector. This is
|
|
|
|
|
// is used to correct the estimated quaternion on the current time step
|
|
|
|
|
// the first 3 states represent the angular misalignment vector.
|
|
|
|
|
// This is used to correct the estimated quaternion on the current time step
|
|
|
|
|
stateStruct.quat.rotate(stateStruct.angErr); |
|
|
|
|
|
|
|
|
|
// record fusion health status
|
|
|
|
|