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

ekf2: initialiseFilter() simplify mag heading init and resetQuatStateYaw

Browse Source 
- most of resetQuatStateYaw doesn't apply to initial heading init, so
removing the special case keeps it simple
This commit is contained in:
Daniel Agar
2022-09-02 08:46:10 -04:00
parent e467d11990
commit 7bbdc220f5
4 changed files with 29 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+17 -17
View File
@@ -176,13 +176,6 @@ bool Ekf::initialiseFilter()
}
}
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
if (_mag_counter < _obs_buffer_length) {
// not enough mag samples accumulated
return false;
}
}
if (_baro_counter < _obs_buffer_length) {
// not enough baro samples accumulated
return false;
@@ -198,18 +191,25 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment
// but do not mark the yaw alignement complete as it needs to be
// reset once the leveling phase is done
if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) && (_mag_counter != 0)) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
if (_mag_counter > 1) {
// rotate the magnetometer measurements into earth frame using a zero yaw angle
// the angle of the projection onto the horizontal gives the yaw angle
const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
// update quaternion states and corresponding covarainces
resetQuatStateYaw(yaw_new, 0.f, false);
// update the rotation matrix using the new yaw value
_R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal));
_state.quat_nominal = _R_to_earth;
// set the earth magnetic field states using the updated rotation
_state.mag_I = _R_to_earth * _mag_lpf.getState();
_state.mag_B.zero();
// set the earth magnetic field states using the updated rotation
_state.mag_I = _R_to_earth * _mag_lpf.getState();
_state.mag_B.zero();
} else {
// not enough mag samples accumulated
return false;
}
}
// initialise the state covariance matrix now we have starting values for all the states