Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: minor spelling fixes

Browse Source
This commit is contained in:
Randy Mackay 2022-01-13 20:45:07 +09:00
parent 88a3bfeccb
commit cff3794d25
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -38,7 +38,7 @@ void NavEKF3_core::SelectFlowFusion()
// Perform tilt check
bool tiltOK = (prevTnb.c.z > frontend->DCM33FlowMin);
// Constrain measurements to zero if takeoff is not detected and the height above ground
// is insuffient to achieve acceptable focus. This allows the vehicle to be picked up
// is insufficient to achieve acceptable focus. This allows the vehicle to be picked up
// and carried to test optical flow operation
if (!takeOffDetected && ((terrainState - stateStruct.position.z) < 0.5f)) {
ofDataDelayed.flowRadXYcomp.zero();
@ -90,13 +90,13 @@ void NavEKF3_core::EstimateTerrainOffset(const of_elements &ofDataDelayed)
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 intialisation 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];
prevPosE = stateStruct.position[1];
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copters vertical velocity
// in addition to a terrain gradient error model, we also have the growth in uncertainty due to the copter's vertical velocity
ftype timeLapsed = MIN(0.001f * (imuSampleTime_ms - timeAtLastAuxEKF_ms), 1.0f);
ftype Pincrement = (distanceTravelledSq * sq(frontend->_terrGradMax)) + sq(timeLapsed)*P[6][6];
Popt += Pincrement;