dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: correct GPS data for antenna position offset

This commit is contained in:
Paul Riseborough 2016-04-09 09:04:53 -07:00
parent 3580940e10
commit e89dbb9f63
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/ekf.cpp
View File

@ -257,6 +257,20 @@ bool Ekf::update()
_fuse_pos = true;
_fuse_vert_vel = true;
_fuse_hor_vel = true;
// correct velocity for offset relative to IMU
Vector3f ang_rate = _imu_sample_delayed.delta_ang * (1.0f/_imu_sample_delayed.delta_ang_dt);
Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body;
Vector3f vel_offset_body = ang_rate % pos_offset_body;
Vector3f vel_offset_earth = _R_prev.transpose() * vel_offset_body;
_gps_sample_delayed.vel -= vel_offset_earth;
// correct position and height for offset relative to IMU
Vector3f pos_offset_earth = _R_prev.transpose() * pos_offset_body;
_gps_sample_delayed.pos(0) -= pos_offset_earth(0);
_gps_sample_delayed.pos(1) -= pos_offset_earth(1);
_gps_sample_delayed.hgt += pos_offset_earth(2);
}
// only use gps height observation in the main filter if specifically enabled