Browse Source

AP_NavEKF2: Change misnomer (NFC)

c415-sdk
murata 4 years ago committed by Peter Barker
parent
commit
a5b6ce9220
  1. 8
      libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
  2. 12
      libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
  3. 4
      libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
  4. 4
      libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
  5. 4
      libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp

8
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp

@ -128,8 +128,8 @@ void NavEKF2_core::FuseAirspeed() @@ -128,8 +128,8 @@ void NavEKF2_core::FuseAirspeed()
statesArray[j] = statesArray[j] - Kfusion[j] * innovVtas;
}
// 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);
// correct the covariance P = (I - K*H)*P
@ -370,8 +370,8 @@ void NavEKF2_core::FuseSideslip() @@ -370,8 +370,8 @@ void NavEKF2_core::FuseSideslip()
statesArray[j] = statesArray[j] - Kfusion[j] * innovBeta;
}
// 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);
// correct the covariance P = (I - K*H)*P

12
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp

@ -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

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@ -697,8 +697,8 @@ void NavEKF2_core::FuseOptFlow() @@ -697,8 +697,8 @@ void NavEKF2_core::FuseOptFlow()
statesArray[j] = statesArray[j] - Kfusion[j] * innovOptFlow[obsIndex];
}
// 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 {

4
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp

@ -891,8 +891,8 @@ void NavEKF2_core::FuseVelPosNED() @@ -891,8 +891,8 @@ void NavEKF2_core::FuseVelPosNED()
statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
}
// the first 3 states represent the angular misalignment vector. This is
// is used to correct the estimated quaternion
// the first 3 states represent the angular misalignment vector.
// This is used to correct the estimated quaternion
stateStruct.quat.rotate(stateStruct.angErr);
// sum the attitude error from velocity and position fusion only

4
libraries/AP_NavEKF2/AP_NavEKF2_RngBcnFusion.cpp

@ -214,8 +214,8 @@ void NavEKF2_core::FuseRngBcn() @@ -214,8 +214,8 @@ void NavEKF2_core::FuseRngBcn()
statesArray[j] = statesArray[j] - Kfusion[j] * innovRngBcn;
}
// 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 healthy fusion

Loading…
Cancel
Save