Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-24 17:48:35 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Fix calculation of predicted LOS rate in terrain estimator

Browse Source
This commit is contained in:
priseborough 2017-06-20 14:22:02 +10:00 committed by Randy Mackay
parent 01f959c1ab
commit 618644addc
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -187,10 +187,10 @@ void NavEKF3_core::EstimateTerrainOffset()
// divide velocity by range, subtract body rates and apply scale factor to // divide velocity by range, subtract body rates and apply scale factor to
// get predicted sensed angular optical rates relative to X and Y sensor axes // get predicted sensed angular optical rates relative to X and Y sensor axes
losPred = relVelSensor.length()/flowRngPred; losPred = norm(relVelSensor.x, relVelSensor.y)/flowRngPred;
// calculate innovations // calculate innovations
auxFlowObsInnov = losPred - sqrtf(sq(ofDataDelayed.flowRadXYcomp[0]) + sq(ofDataDelayed.flowRadXYcomp[1])); auxFlowObsInnov = losPred - norm(ofDataDelayed.flowRadXYcomp.x, ofDataDelayed.flowRadXYcomp.y);
// calculate observation jacobian // calculate observation jacobian
float t3 = sq(q0); float t3 = sq(q0);