Browse Source

AP_NavEKF2: minor spelling fixes to optflow fusion

gps-1.3.1
Randy Mackay 3 years ago
parent
commit
6c947fc10b
  1. 4
      libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

4
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp

@ -89,7 +89,7 @@ void NavEKF2_core::EstimateTerrainOffset() @@ -89,7 +89,7 @@ void NavEKF2_core::EstimateTerrainOffset()
gndHgtValidTime_ms = imuSampleTime_ms;
// propagate ground position state noise each time this is called using the difference in position since the last observations and an RMS gradient assumption
// limit distance to prevent intialisation afer bad gps causing bad numerical conditioning
// limit distance to prevent initialisation after bad gps causing bad numerical conditioning
ftype distanceTravelledSq = sq(stateStruct.position[0] - prevPosN) + sq(stateStruct.position[1] - prevPosE);
distanceTravelledSq = MIN(distanceTravelledSq, 100.0f);
prevPosN = stateStruct.position[0];
@ -303,7 +303,7 @@ void NavEKF2_core::FuseOptFlow() @@ -303,7 +303,7 @@ void NavEKF2_core::FuseOptFlow()
// Fuse X and Y axis measurements sequentially assuming observation errors are uncorrelated
for (uint8_t obsIndex=0; obsIndex<=1; obsIndex++) { // fuse X axis data first
// calculate range from ground plain to centre of sensor fov assuming flat earth
// calculate range from ground plane to centre of sensor fov assuming flat earth
ftype range = constrain_ftype((heightAboveGndEst/prevTnb.c.z),rngOnGnd,1000.0f);
// correct range for flow sensor offset body frame position offset

Loading…
Cancel
Save