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

AP_NavEKF3: add CalculateVelInnovationsAndVariances

This commit is contained in:
Randy Mackay 2020-10-12 09:04:45 +09:00
parent 2505fd5c1c
commit 6b2b5c4ca0
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -388,6 +388,19 @@ void NavEKF3_core::CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav
#endif
}
// calculate velocity variance helper function
void NavEKF3_core::CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const
{
// innovations are latest estimate - latest observation
innovations = stateStruct.velocity - velocity;
const float obs_data_chk = sq(constrain_float(noise, 0.05f, 5.0f)) + sq(accel_scale * accNavMag);
// calculate innovation variance. velocity states start at index 4
variances.x = P[4][4] + obs_data_chk;
variances.y = P[5][5] + obs_data_chk;
variances.z = P[6][6] + obs_data_chk;
}
/********************************************************
* FUSE MEASURED_DATA *

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -954,6 +954,12 @@ private:
// correct external navigation earth-frame velocity using sensor body-frame offset
void CorrectExtNavVelForSensorOffset(ext_nav_vel_elements &ext_nav_vel_data) const;
// calculate velocity variances and innovations
// Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
// Scale factor applied to vertical velocity measurement variance due to manoeuvre acceleration
// variances argument is updated with variances for each axis
void CalculateVelInnovationsAndVariances(const Vector3f &velocity, float noise, float accel_scale, Vector3f &innovations, Vector3f &variances) const;
// Runs the IMU prediction step for an independent GSF yaw estimator algorithm
// that uses IMU, GPS horizontal velocity and optionally true airspeed data.
void runYawEstimatorPrediction(void);