Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Soaring: Adjust initial EKF values and limit R to 40.0m.

This commit is contained in:
Samuel Tabor 2019-07-07 13:10:22 +01:00 committed by Andrew Tridgell
parent ff1725b549
commit fdf7eae01c
2 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Soaring/AP_Soaring.h
View File

@ -17,10 +17,10 @@
#include <AP_SpdHgtControl/AP_SpdHgtControl.h>
#define INITIAL_THERMAL_STRENGTH 2.0
#define INITIAL_THERMAL_RADIUS 30.0 //150.0
#define INITIAL_THERMAL_RADIUS 80.0
#define INITIAL_STRENGTH_COVARIANCE 0.0049
#define INITIAL_RADIUS_COVARIANCE 2500.0
#define INITIAL_POSITION_COVARIANCE 300.0
#define INITIAL_RADIUS_COVARIANCE 400.0
#define INITIAL_POSITION_COVARIANCE 400.0
class SoaringController {

2
libraries/AP_Soaring/ExtendedKalmanFilter.cpp
View File

@ -69,7 +69,7 @@ void ExtendedKalmanFilter::update(float z, float Px, float Py, float driftX, flo
X += K * (z - z1);
// Make sure X[1] stays positive.
X[1] = X[1]>0 ? X[1]: -X[1];
X[1] = X[1]>40.0 ? X[1]: 40.0;
// Correct the covariance too.
// LINE 46