fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 03:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: use estimator's estimate of variance for standard deviation calculation

Browse Source
This commit is contained in:
bugobliterator
2016-03-13 17:47:56 +01:00
committed by Lorenz Meier
parent c02d17f56e
commit d33dbb8cbb
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+8 -4
View File
@@ -497,8 +497,12 @@ void Ekf2::task_main()
// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres // TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres
// TODO: Should use sqrt of filter position variances // TODO: Should use sqrt of filter position variances
lpos.eph = gps.eph; // get pos vel state variance
lpos.epv = gps.epv; Vector3f pos_var, vel_var;
_ekf->get_pos_var(pos_var);
_ekf->get_vel_var(vel_var);
lpos.eph = sqrt(pos_var(0)+pos_var(1));
lpos.epv = sqrt(pos_var(2));
// publish vehicle local position data // publish vehicle local position data
if (_lpos_pub == nullptr) { if (_lpos_pub == nullptr) {
@@ -569,8 +573,8 @@ void Ekf2::task_main()
global_pos.yaw = euler(2); // Yaw in radians -PI..+PI. global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally global_pos.eph = sqrt(pos_var(0)+pos_var(1));; // Standard deviation of position estimate horizontally
global_pos.epv = gps.epv; // Standard deviation of position vertically global_pos.epv = sqrt(pos_var(2)); // Standard deviation of position vertically
// TODO: implement terrain estimator // TODO: implement terrain estimator
global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84 global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84