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

EKF: test new derivation

Browse Source 
Use direct attitude parameterisation
Discard scale factors
Add accel bias
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 22d18d638c
commit fe9f88a8b4
12 changed files with 928 additions and 1498 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/vel_pos_fusion.cpp
+2 -17
View File
@@ -196,7 +196,7 @@ void Ekf::fuseVelPosHeight()
continue;
}
unsigned state_index = obs_index + 3; // we start with vx and this is the 4. state
unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row <= 15; row++) {
@@ -227,24 +227,9 @@ void Ekf::fuseVelPosHeight()
}
}
// sum the attitude error from velocity and position fusion only
// used as a metric for convergence monitoring
if (obs_index != 5) {
_tilt_err_vec += _state.ang_error;
}
// by definition the angle error state is zero at the fusion time
_state.ang_error.setZero();
// fuse the observation
// update the states
fuse(Kfusion, _vel_pos_innov[obs_index]);
// correct the nominal quaternion
Quaternion dq;
dq.from_axis_angle(_state.ang_error);
_state.quat_nominal = dq * _state.quat_nominal;
_state.quat_nominal.normalize();
// update covarinace matrix via Pnew = (I - KH)P
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {