fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Add functions to get position and velocity state variance

Browse Source
This commit is contained in:
Paul Riseborough 2016-03-07 20:22:06 +11:00
parent 32b03819ef
commit 2c2850c0ce
1 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
EKF/covariance.cpp
View File

@ -103,6 +103,20 @@ void Ekf::initialiseCovariance()
}
void Ekf::get_pos_var(Vector3f &pos_var)
{
pos_var(0) = P[6][6];
pos_var(1) = P[7][7];
pos_var(2) = P[8][8];
}
void Ekf::get_vel_var(Vector3f &vel_var)
{
vel_var(0) = P[3][3];
vel_var(1) = P[4][4];
vel_var(2) = P[5][5];
}
void Ekf::predictCovariance()
{
// assign intermediate state variables