5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 01:48:29 -04:00

AP_NavEKF3: minor format and spelling fixes

This commit is contained in:
Randy Mackay 2020-10-21 13:45:13 +09:00
parent 231a958e51
commit e4056086e0
2 changed files with 3 additions and 6 deletions

View File

@ -275,7 +275,7 @@ void NavEKF3_core::setAidingMode()
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false;
if (!posAiding ) {
if (!posAiding) {
uint16_t maxLossTime_ms;
if (!velAiding) {
maxLossTime_ms = frontend->posRetryTimeNoVel_ms;

View File

@ -850,7 +850,7 @@ void NavEKF3_core::FuseVelPosNED()
Kfusion[i] = P[i][stateIndex]*SK;
}
// inhibit delta angle bias state estmation by setting Kalman gains to zero
// inhibit delta angle bias state estimation by setting Kalman gains to zero
if (!inhibitDelAngBiasStates) {
for (uint8_t i = 10; i<=12; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
@ -892,8 +892,7 @@ void NavEKF3_core::FuseVelPosNED()
// update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
// this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++)
{
for (uint8_t j= 0; j<=stateIndexLim; j++) {
KHP[i][j] = Kfusion[i] * P[stateIndex][j];
}
}
@ -1838,9 +1837,7 @@ void NavEKF3_core::SelectBodyOdomFusion()
// Fuse data into the main filter
FuseBodyVel();
}
}
}