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

EKF2: rotate vel on heading reset (#25753)

Browse Source 
* correct NED velocity according to yaw reset to keep "body-velocity" consistent
* add unit test velocityRotationOnYawReset
* '[AUTO COMMIT] update change indication'
* dont adjust velocity on yaw-reset when NE aiding active

---------

Co-authored-by: haumarco <haumarco@users.noreply.github.com>
This commit is contained in:
Marco Hauswirth 2025-10-21 11:48:03 +02:00 committed by GitHub
parent 71f56df23b
commit 59f6c69067
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
5 changed files with 94 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/ekf2/EKF/ekf.h
View File

@ -1019,6 +1019,7 @@ private:
void resetQuatStateYaw(float yaw, float yaw_variance);
void propagateQuatReset(const Quatf &quat_before_reset);
void resetYawByFusion(float yaw, float yaw_variance);
void resetHorizontalVelocityToMatchYaw(float delta_yaw);
HeightSensor _height_sensor_ref{HeightSensor::UNKNOWN};
PositionSensor _position_sensor_ref{PositionSensor::GNSS};

16
src/modules/ekf2/EKF/yaw_fusion.cpp
View File

@ -140,6 +140,10 @@ void Ekf::resetQuatStateYaw(const float yaw, const float yaw_variance)
_time_last_heading_fuse = _time_delayed_us;
propagateQuatReset(quat_before_reset);
// rotate horizontal velocity by the yaw change
const float yaw_diff = wrap_pi(yaw - getEulerYaw(quat_before_reset));
resetHorizontalVelocityToMatchYaw(yaw_diff);
}
void Ekf::propagateQuatReset(const Quatf &quat_before_reset)
@ -189,4 +193,16 @@ void Ekf::resetYawByFusion(const float yaw, const float yaw_variance)
fuseYaw(aid_src_status, H_YAW, reset_yaw);
propagateQuatReset(quat_before_reset);
resetHorizontalVelocityToMatchYaw(-aid_src_status.innovation);
}
void Ekf::resetHorizontalVelocityToMatchYaw(const float delta_yaw)
{
if (!isNorthEastAidingActive() && fabsf(delta_yaw) > 0.3f) {
const matrix::Dcm2f R_yaw(delta_yaw);
const Vector2f vel_rotated = R_yaw * Vector2f(_state.vel);
const float vel_var = fmaxf(P(State::vel.idx, State::vel.idx), P(State::vel.idx + 1, State::vel.idx + 1));
resetHorizontalVelocityTo(vel_rotated, vel_var);
}
}

32
src/modules/ekf2/test/change_indication/ekf_gsf_reset.csv
View File

@ -67,22 +67,22 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0057,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
6590000,1,-0.0093,-0.011,0.00016,0.0037,0.0057,-0.099,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
6690000,1,-0.0093,-0.011,9.4e-05,0.0046,0.0053,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0052,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7
6890000,0.71,0.0013,-0.014,0.7,0.00052,0.0065,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8
6990000,0.71,0.0013,-0.014,0.71,-0.00025,0.0048,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8
7090000,0.71,0.0012,-0.014,0.71,-0.00081,0.0013,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8
7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0021,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8
7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.01,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.012,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7590000,0.71,0.0017,-0.014,0.71,-0.0034,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7690000,0.71,0.0017,-0.014,0.71,-0.0046,0.018,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2
7790000,0.71,0.0017,-0.014,0.71,-0.011,0.017,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2
7890000,0.71,0.0017,-0.014,0.71,-0.011,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2
7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.023,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2
8090000,0.71,0.0017,-0.014,0.71,-0.0051,0.025,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8190000,0.71,0.0018,-0.014,0.71,-0.014,0.029,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8290000,0.71,0.0017,-0.014,0.71,-0.018,0.023,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
6790000,0.71,0.0012,-0.014,0.71,-0.0056,0.0034,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.1,0.1,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7
6890000,0.71,0.0013,-0.014,0.7,-0.0076,0.0039,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.18,0.18,0.46,0.1,0.1,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8
6990000,0.71,0.0013,-0.014,0.71,-0.0078,0.0042,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.19,0.19,0.36,0.11,0.11,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8
7090000,0.71,0.0012,-0.014,0.71,-0.0084,0.0028,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.2,0.2,0.29,0.12,0.12,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8
7190000,0.71,0.0013,-0.014,0.71,-0.01,0.0027,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.21,0.21,0.24,0.14,0.14,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8
7290000,0.71,0.0015,-0.014,0.71,-0.012,0.0052,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.22,0.23,0.2,0.16,0.16,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7390000,0.71,0.0016,-0.014,0.71,-0.012,0.0081,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.24,0.25,0.18,0.18,0.18,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7490000,0.71,0.0015,-0.014,0.71,-0.013,0.0071,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.27,0.27,0.15,0.21,0.21,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7590000,0.71,0.0017,-0.014,0.71,-0.014,0.012,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.29,0.29,0.14,0.25,0.25,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9
7690000,0.71,0.0017,-0.014,0.71,-0.015,0.011,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.32,0.32,0.13,0.29,0.29,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2
7790000,0.71,0.0017,-0.014,0.71,-0.019,0.011,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.36,0.36,0.12,0.34,0.34,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2
7890000,0.71,0.0017,-0.014,0.71,-0.02,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.39,0.39,0.11,0.4,0.4,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2
7990000,0.71,0.0017,-0.014,0.71,-0.02,0.017,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.43,0.43,0.1,0.47,0.47,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2
8090000,0.71,0.0017,-0.014,0.71,-0.018,0.019,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.47,0.47,0.1,0.55,0.55,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8190000,0.71,0.0018,-0.014,0.71,-0.024,0.021,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.52,0.51,0.099,0.64,0.64,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8290000,0.71,0.0017,-0.014,0.71,-0.027,0.018,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.56,0.55,0.097,0.74,0.74,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00048,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00052,0.00039,-0.021,0.21,-9.6e-06,0.43,-0.00031,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
8490000,0.71,0.0017,-0.014,0.71,-0.0029,0.0026,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00052,0.00039,-0.025,0.21,-9.3e-06,0.43,-0.00032,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2
8590000,0.71,0.0022,-0.014,0.71,-0.00042,0.00097,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-7e-05,-0.00052,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00043,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2

1 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
67 6490000,1,-0.0093,-0.011,0.00023,0.0049,0.0057,-0.052,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,0,0,-0.00015,0,0,0,0,0,0,0,0,-4.9e+02,0.0018,0.0018,0.0001,0.26,0.26,1.5,0.15,0.15,0.26,0.00012,0.00012,2.8e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
68 6590000,1,-0.0093,-0.011,0.00016,0.0037,0.0057,-0.099,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,2.9e-05,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.2,0.2,1.1,0.12,0.12,0.23,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
69 6690000,1,-0.0093,-0.011,9.4e-05,0.0046,0.0053,-0.076,0,0,-4.9e+02,-0.0014,-0.0057,-7.6e-05,0,0,-0.00029,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.78,0.14,0.14,0.21,9.7e-05,9.7e-05,2.6e-06,0.04,0.04,0.04,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
70 6790000,0.71,0.0012,-0.014,0.71,0.0049,0.0052,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.11,0.11,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7 6790000,0.71,0.0012,-0.014,0.71,-0.0056,0.0034,-0.11,0,0,-4.9e+02,-0.0014,-0.0057,-7.5e-05,0,0,-7e-05,0.21,8e-05,0.43,3.5e-05,0.00033,-0.0024,0,0,-4.9e+02,0.0013,0.0013,0.053,0.18,0.18,0.6,0.1,0.1,0.2,7.8e-05,7.7e-05,2.4e-06,0.04,0.04,0.04,0.0015,0.0012,0.0014,0.0018,0.0015,0.0014,1,1,1.7
71 6890000,0.71,0.0013,-0.014,0.7,0.00052,0.0065,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.21,0.21,0.46,0.13,0.14,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8 6890000,0.71,0.0013,-0.014,0.7,-0.0076,0.0039,-0.12,0,0,-4.9e+02,-0.0015,-0.0057,-7.6e-05,0,0,-9.8e-05,0.21,1.3e-05,0.43,6.1e-07,0.00089,-0.00086,0,0,-4.9e+02,0.0013,0.0013,0.047,0.18,0.18,0.46,0.1,0.1,0.18,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0014,0.00065,0.0013,0.0017,0.0014,0.0013,1,1,1.8
72 6990000,0.71,0.0013,-0.014,0.71,-0.00025,0.0048,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.23,0.24,0.36,0.17,0.17,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8 6990000,0.71,0.0013,-0.014,0.71,-0.0078,0.0042,-0.12,0,0,-4.9e+02,-0.0014,-0.0057,-7.7e-05,-2.8e-05,-0.00025,-0.00041,0.21,-3.9e-05,0.43,-0.00025,0.00056,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.044,0.19,0.19,0.36,0.11,0.11,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00044,0.0013,0.0017,0.0013,0.0013,1,1,1.8
73 7090000,0.71,0.0012,-0.014,0.71,-0.00081,0.0013,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.27,0.27,0.29,0.2,0.21,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8 7090000,0.71,0.0012,-0.014,0.71,-0.0084,0.0028,-0.13,0,0,-4.9e+02,-0.0014,-0.0057,-7.9e-05,0.0002,-0.00056,-0.00078,0.21,-3.8e-05,0.43,-0.00038,0.00025,-0.00041,0,0,-4.9e+02,0.0013,0.0013,0.043,0.2,0.2,0.29,0.12,0.12,0.16,7.8e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00034,0.0013,0.0017,0.0013,0.0013,1,1,1.8
74 7190000,0.71,0.0013,-0.014,0.71,-0.0025,0.0021,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.3,0.31,0.24,0.25,0.25,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8 7190000,0.71,0.0013,-0.014,0.71,-0.01,0.0027,-0.15,0,0,-4.9e+02,-0.0014,-0.0057,-7.8e-05,0.00014,-0.0005,-0.00057,0.21,-3.1e-05,0.43,-0.00036,0.00035,-0.00045,0,0,-4.9e+02,0.0013,0.0013,0.042,0.21,0.21,0.24,0.14,0.14,0.15,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00027,0.0013,0.0016,0.0013,0.0013,1,1,1.8
75 7290000,0.71,0.0015,-0.014,0.71,-0.0044,0.01,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.34,0.35,0.2,0.3,0.31,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9 7290000,0.71,0.0015,-0.014,0.71,-0.012,0.0052,-0.15,0,0,-4.9e+02,-0.0015,-0.0057,-7.5e-05,-0.00022,-0.00021,-0.0012,0.21,-3.3e-05,0.43,-0.00035,0.00077,-0.00038,0,0,-4.9e+02,0.0013,0.0013,0.042,0.22,0.23,0.2,0.16,0.16,0.14,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.04,0.0013,0.00023,0.0013,0.0016,0.0013,0.0013,1,1,1.9
76 7390000,0.71,0.0016,-0.014,0.71,-0.0037,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.38,0.39,0.18,0.37,0.37,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9 7390000,0.71,0.0016,-0.014,0.71,-0.012,0.0081,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.3e-05,-0.00029,-4.1e-05,-0.0014,0.21,-2.9e-05,0.43,-0.00035,0.00086,-0.00031,0,0,-4.9e+02,0.0013,0.0013,0.041,0.24,0.25,0.18,0.18,0.18,0.13,7.7e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.0002,0.0013,0.0016,0.0013,0.0013,1,1,1.9
77 7490000,0.71,0.0015,-0.014,0.71,-0.0038,0.012,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.43,0.43,0.15,0.44,0.45,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9 7490000,0.71,0.0015,-0.014,0.71,-0.013,0.0071,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00019,-7.7e-05,-0.0022,0.21,-2.2e-05,0.43,-0.00032,0.00079,-0.00042,0,0,-4.9e+02,0.0013,0.0013,0.041,0.27,0.27,0.15,0.21,0.21,0.12,7.6e-05,7.6e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00017,0.0013,0.0016,0.0013,0.0013,1,1,1.9
78 7590000,0.71,0.0017,-0.014,0.71,-0.0034,0.021,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.48,0.48,0.14,0.53,0.53,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9 7590000,0.71,0.0017,-0.014,0.71,-0.014,0.012,-0.17,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00024,0.0002,-0.003,0.21,-2.1e-05,0.43,-0.00035,0.00087,-0.00026,0,0,-4.9e+02,0.0013,0.0013,0.04,0.29,0.29,0.14,0.25,0.25,0.12,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00015,0.0013,0.0016,0.0013,0.0013,1,1,1.9
79 7690000,0.71,0.0017,-0.014,0.71,-0.0046,0.018,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.53,0.53,0.13,0.63,0.63,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2 7690000,0.71,0.0017,-0.014,0.71,-0.015,0.011,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.1e-05,-0.00025,0.00018,-0.0051,0.21,-1.8e-05,0.43,-0.00033,0.00084,-0.00033,0,0,-4.9e+02,0.0014,0.0013,0.04,0.32,0.32,0.13,0.29,0.29,0.11,7.5e-05,7.5e-05,2.4e-06,0.04,0.04,0.039,0.0013,0.00014,0.0013,0.0016,0.0013,0.0013,1,1,2
80 7790000,0.71,0.0017,-0.014,0.71,-0.011,0.017,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.58,0.58,0.12,0.74,0.74,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2 7790000,0.71,0.0017,-0.014,0.71,-0.019,0.011,-0.16,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00037,5.8e-05,-0.0071,0.21,-1.7e-05,0.43,-0.00034,0.00079,-0.00038,0,0,-4.9e+02,0.0014,0.0013,0.04,0.36,0.36,0.12,0.34,0.34,0.11,7.4e-05,7.5e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00013,0.0013,0.0016,0.0013,0.0013,1,1,2
81 7890000,0.71,0.0017,-0.014,0.71,-0.011,0.022,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.64,0.63,0.11,0.87,0.86,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2 7890000,0.71,0.0017,-0.014,0.71,-0.02,0.015,-0.16,0,0,-4.9e+02,-0.0016,-0.0057,-7.2e-05,-0.00039,0.0002,-0.0096,0.21,-1.6e-05,0.43,-0.00035,0.00079,-0.0003,0,0,-4.9e+02,0.0014,0.0014,0.04,0.39,0.39,0.11,0.4,0.4,0.1,7.3e-05,7.4e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00012,0.0013,0.0016,0.0013,0.0013,1,1,2
82 7990000,0.71,0.0017,-0.014,0.71,-0.0091,0.023,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.7,0.68,0.1,1,0.99,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2 7990000,0.71,0.0017,-0.014,0.71,-0.02,0.017,-0.16,0,0,-4.9e+02,-0.0016,-0.0056,-7.1e-05,-0.00038,0.00028,-0.011,0.21,-1.4e-05,0.43,-0.00035,0.00086,-0.00038,0,0,-4.9e+02,0.0014,0.0014,0.04,0.43,0.43,0.1,0.47,0.47,0.099,7.1e-05,7.3e-05,2.4e-06,0.04,0.04,0.038,0.0013,0.00011,0.0013,0.0016,0.0013,0.0013,1,1,2
83 8090000,0.71,0.0017,-0.014,0.71,-0.0051,0.025,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.76,0.74,0.1,1.2,1.1,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1 8090000,0.71,0.0017,-0.014,0.71,-0.018,0.019,-0.17,0,0,-4.9e+02,-0.0016,-0.0056,-6.9e-05,-0.00031,0.00035,-0.011,0.21,-1.2e-05,0.43,-0.00034,0.0009,-0.00035,0,0,-4.9e+02,0.0014,0.0014,0.04,0.47,0.47,0.1,0.55,0.55,0.097,7e-05,7.3e-05,2.4e-06,0.04,0.04,0.037,0.0013,0.0001,0.0013,0.0016,0.0013,0.0013,1,1,2.1
84 8190000,0.71,0.0018,-0.014,0.71,-0.014,0.029,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.82,0.79,0.099,1.4,1.3,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 8190000,0.71,0.0018,-0.014,0.71,-0.024,0.021,-0.18,0,0,-4.9e+02,-0.0016,-0.0057,-7e-05,-0.00045,0.00036,-0.013,0.21,-1.2e-05,0.43,-0.00036,0.00085,-0.00036,0,0,-4.9e+02,0.0014,0.0014,0.04,0.52,0.51,0.099,0.64,0.64,0.094,6.8e-05,7.2e-05,2.4e-06,0.04,0.04,0.037,0.0013,9.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
85 8290000,0.71,0.0017,-0.014,0.71,-0.018,0.023,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.88,0.84,0.097,1.6,1.5,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1 8290000,0.71,0.0017,-0.014,0.71,-0.027,0.018,-0.17,0,0,-4.9e+02,-0.0015,-0.0057,-7.4e-05,-0.00057,0.00031,-0.017,0.21,-1e-05,0.43,-0.00032,0.00079,-0.00034,0,0,-4.9e+02,0.0014,0.0014,0.039,0.56,0.55,0.097,0.74,0.74,0.091,6.6e-05,7.1e-05,2.4e-06,0.04,0.04,0.036,0.0013,8.7e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
86 8390000,0.71,0.0017,-0.014,0.71,-0.0012,0.00048,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.1e-05,-0.00052,0.00039,-0.021,0.21,-9.6e-06,0.43,-0.00031,0.00084,-0.00031,0,0,-4.9e+02,0.0014,0.0014,0.039,25,25,0.097,1e+02,1e+02,0.091,6.4e-05,7e-05,2.4e-06,0.04,0.04,0.035,0.0013,8.3e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.1
87 8490000,0.71,0.0017,-0.014,0.71,-0.0029,0.0026,-0.17,0,0,-4.9e+02,-0.0015,-0.0056,-7.2e-05,-0.00052,0.00039,-0.025,0.21,-9.3e-06,0.43,-0.00032,0.0008,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.096,1e+02,1e+02,0.089,6.2e-05,6.9e-05,2.4e-06,0.04,0.04,0.034,0.0013,7.8e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2
88 8590000,0.71,0.0022,-0.014,0.71,-0.00042,0.00097,-0.17,0,0,-4.9e+02,-0.0017,-0.0057,-7e-05,-0.00052,0.00039,-0.029,0.21,-1.2e-05,0.43,-0.00043,0.00076,-0.00028,0,0,-4.9e+02,0.0013,0.0014,0.039,25,25,0.095,1e+02,1e+02,0.088,6e-05,6.8e-05,2.4e-06,0.04,0.04,0.033,0.0013,7.4e-05,0.0013,0.0016,0.0013,0.0013,1,1,2.2

8
src/modules/ekf2/test/change_indication/iris_gps.csv
View File

@ -69,10 +69,10 @@ Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7
6690000,1,-0.0088,-0.011,0.00052,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
6790000,1,-0.0089,-0.011,0.00049,0.003,0.016,-0.043,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
6890000,1,-0.0087,-0.011,0.0004,0.0023,0.016,-0.039,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8
6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8
7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8
7190000,0.98,-0.0065,-0.012,0.18,-6.6e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8
7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9
6990000,0.98,-0.0067,-0.012,0.18,-0.0032,0.013,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.16,0.031,0.097,0.097,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8
7090000,0.98,-0.0065,-0.012,0.18,-0.0041,0.017,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.16,0.16,0.03,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8
7190000,0.98,-0.0065,-0.012,0.18,-0.0046,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.16,0.16,0.029,0.11,0.11,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8
7290000,0.98,-0.0064,-0.012,0.18,-0.0041,0.023,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.17,0.17,0.028,0.12,0.12,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9
7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00095,-0.032,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.9e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9
7490000,0.98,-0.0063,-0.012,0.18,0.00098,0.0035,-0.026,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.8e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9
7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.0061,-0.023,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00035,-0.00039,-8.8e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9

1 Timestamp,state[0],state[1],state[2],state[3],state[4],state[5],state[6],state[7],state[8],state[9],state[10],state[11],state[12],state[13],state[14],state[15],state[16],state[17],state[18],state[19],state[20],state[21],state[22],state[23],variance[0],variance[1],variance[2],variance[3],variance[4],variance[5],variance[6],variance[7],variance[8],variance[9],variance[10],variance[11],variance[12],variance[13],variance[14],variance[15],variance[16],variance[17],variance[18],variance[19],variance[20],variance[21],variance[22],variance[23]
69 6690000,1,-0.0088,-0.011,0.00052,0.0022,0.018,-0.044,0,0,-4.9e+02,-0.0017,-0.0056,-9e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0016,0.0016,9.9e-05,0.23,0.23,0.035,0.14,0.14,0.068,9.8e-05,9.8e-05,2.6e-06,0.04,0.04,0.0031,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
70 6790000,1,-0.0089,-0.011,0.00049,0.003,0.016,-0.043,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0014,0.0014,9.3e-05,0.18,0.18,0.034,0.11,0.11,0.068,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.003,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.7
71 6890000,1,-0.0087,-0.011,0.0004,0.0023,0.016,-0.039,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,0,0,-0.13,0,0,0,0,0,0,0,0,-4.9e+02,0.0015,0.0015,9.6e-05,0.21,0.21,0.032,0.14,0.14,0.067,8e-05,8.1e-05,2.4e-06,0.04,0.04,0.0028,0.0025,0.0025,0.0025,0.0025,0.0025,0.0025,1,1,1.8
72 6990000,0.98,-0.0067,-0.012,0.18,0.0025,0.011,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.17,0.031,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8 6990000,0.98,-0.0067,-0.012,0.18,-0.0032,0.013,-0.037,0,0,-4.9e+02,-0.0015,-0.0056,-9.4e-05,0,0,-0.13,0.21,-0.00049,0.44,0.00044,-0.0011,0.00036,0,0,-4.9e+02,0.0012,0.0012,0.054,0.16,0.16,0.031,0.097,0.097,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0026,0.0015,0.0012,0.0014,0.0015,0.0018,0.0014,1,1,1.8
73 7090000,0.98,-0.0065,-0.012,0.18,0.00012,0.018,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.18,0.19,0.03,0.13,0.13,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8 7090000,0.98,-0.0065,-0.012,0.18,-0.0041,0.017,-0.038,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,0,0,-0.13,0.2,-0.00015,0.44,-0.00019,-0.00047,0.00017,0,0,-4.9e+02,0.0013,0.0013,0.048,0.16,0.16,0.03,0.1,0.1,0.066,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0024,0.0014,0.00067,0.0013,0.0014,0.0016,0.0013,1,1,1.8
74 7190000,0.98,-0.0065,-0.012,0.18,-6.6e-05,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.21,0.21,0.029,0.16,0.16,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8 7190000,0.98,-0.0065,-0.012,0.18,-0.0046,0.019,-0.037,0,0,-4.9e+02,-0.0016,-0.0056,-9.4e-05,-6.3e-05,3.4e-05,-0.13,0.2,-0.0001,0.43,-0.00018,-0.00052,-3.3e-06,0,0,-4.9e+02,0.0013,0.0013,0.046,0.16,0.16,0.029,0.11,0.11,0.065,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0023,0.0013,0.00044,0.0013,0.0014,0.0016,0.0013,1,1,1.8
75 7290000,0.98,-0.0064,-0.012,0.18,-0.00068,0.024,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.24,0.24,0.028,0.19,0.19,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9 7290000,0.98,-0.0064,-0.012,0.18,-0.0041,0.023,-0.034,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.0003,0.00019,-0.13,0.2,-7.3e-05,0.43,-0.00037,-0.00044,6.5e-05,0,0,-4.9e+02,0.0014,0.0013,0.044,0.17,0.17,0.028,0.12,0.12,0.064,6.5e-05,6.4e-05,2.2e-06,0.04,0.04,0.0022,0.0013,0.00033,0.0013,0.0014,0.0016,0.0013,1,1,1.9
76 7390000,0.98,-0.0063,-0.012,0.18,-0.0015,0.00095,-0.032,0,0,-4.9e+02,-0.0016,-0.0057,-9.4e-05,-0.00036,0.00036,-0.13,0.2,-5.7e-05,0.43,-0.00048,-0.0004,8.9e-05,0,0,-4.9e+02,0.0014,0.0014,0.043,25,25,0.027,1e+02,1e+02,0.064,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.002,0.0013,0.00027,0.0013,0.0014,0.0016,0.0013,1,1,1.9
77 7490000,0.98,-0.0063,-0.012,0.18,0.00098,0.0035,-0.026,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-4.5e-05,0.43,-0.00042,-0.00038,-7.8e-05,0,0,-4.9e+02,0.0015,0.0014,0.043,25,25,0.026,1e+02,1e+02,0.063,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0019,0.0013,0.00022,0.0013,0.0014,0.0016,0.0013,1,1,1.9
78 7590000,0.98,-0.0064,-0.012,0.18,0.0021,0.0061,-0.023,0,0,-4.9e+02,-0.0016,-0.0056,-9.2e-05,-0.00036,0.00036,-0.13,0.2,-3.8e-05,0.43,-0.00035,-0.00039,-8.8e-06,0,0,-4.9e+02,0.0015,0.0015,0.042,25,25,0.025,51,51,0.062,6.4e-05,6.3e-05,2.2e-06,0.04,0.04,0.0018,0.0013,0.00019,0.0013,0.0014,0.0016,0.0013,1,1,1.9

57
src/modules/ekf2/test/test_EKF_mag.cpp
View File

@ -214,3 +214,60 @@ TEST_F(EkfMagTest, suddenInclinationChange)
EXPECT_FALSE(_ekf_wrapper.isIntendingMagHeadingFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingMag3DFusion());
}
TEST_F(EkfMagTest, velocityRotationOnYawReset)
{
// GIVEN: Mag fusion is active and vehicle is flying with airspeed
const float initial_mag_heading = M_PI_F / 4.f; // 45 degrees
Vector3f mag_data(0.2f * cosf(initial_mag_heading), -0.2f * sinf(initial_mag_heading), 0.4f);
_sensor_simulator._mag.setData(mag_data);
_sensor_simulator.runSeconds(_init_duration_s);
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_ekf->set_is_fixed_wing(true);
const float airspeed_body = 15.0f; // 15 m/s airspeed in body X direction
_sensor_simulator.startAirspeedSensor();
_sensor_simulator._airspeed.setData(airspeed_body, airspeed_body);
_ekf_wrapper.enableBetaFusion();
_sensor_simulator.runSeconds(3);
// initial state
const Vector3f vel_before = _ekf->getVelocity();
const float yaw_before = _ekf_wrapper.getYawAngle();
const matrix::Dcm2f R_ned_to_body_before(-yaw_before);
const Vector2f vel_body_before = R_ned_to_body_before * Vector2f(vel_before);
// WHEN: Mag heading suddenly changes by more than 0.3 rad (90 degrees)
const float new_mag_heading = yaw_before + M_PI_F / 2.f;
mag_data = Vector3f(0.2f * cosf(new_mag_heading), -0.2f * sinf(new_mag_heading), 0.4f);
_sensor_simulator._mag.setData(mag_data);
_sensor_simulator.runSeconds(8.f);
// THEN: the yaw should be reset to the new mag heading
const float yaw_after = _ekf_wrapper.getYawAngle();
EXPECT_NEAR(yaw_after, new_mag_heading, radians(5.0f))
<< "Yaw after: " << degrees(yaw_after)
<< " Expected: " << degrees(new_mag_heading);
// AND: the NED velocity should be rotated to maintain consistent body-frame velocity
const Vector3f vel_after = _ekf->getVelocity();
// Calculate body-frame velocity after reset
const matrix::Dcm2f R_ned_to_body_after(-yaw_after);
const Vector2f vel_body_after = R_ned_to_body_after * Vector2f(vel_after);
// Body-frame velocity should remain approximately the same
EXPECT_NEAR(vel_body_before(0), vel_body_after(0), 1.0f)
<< "Body-frame velocity X before: " << vel_body_before(0)
<< " after: " << vel_body_after(0);
EXPECT_NEAR(vel_body_before(1), vel_body_after(1), 1.0f)
<< "Body-frame velocity Y before: " << vel_body_before(1)
<< " after: " << vel_body_after(1);
// Verify that the yaw change was sufficient to trigger velocity rotation (> 0.3 rad)
const float yaw_change = fabsf(wrap_pi(yaw_after - yaw_before));
EXPECT_GT(yaw_change, 0.3f) << "Yaw change: " << degrees(yaw_change) << " deg";
}