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

ekf2: verbose debug print status include state variances

This commit is contained in:
Daniel Agar 2024-01-11 11:53:20 -05:00 committed by GitHub
parent 4cb293020a
commit 8a031677d5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 42 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/modules/ekf2/EKF/ekf.cpp
View File

@ -379,34 +379,58 @@ static void printRingBuffer(const char *name, RingBuffer<T> *rb)
void Ekf::print_status()
{
printf("\nStates: (%.4f seconds ago)\n", (_time_latest_us - _time_delayed_us) * 1e-6);
printf("Orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n",
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2),
(double)_state.quat_nominal(3),
(double)matrix::Eulerf(_state.quat_nominal).phi(), (double)matrix::Eulerf(_state.quat_nominal).theta(),
(double)matrix::Eulerf(_state.quat_nominal).psi());
printf("Orientation (%d-%d): [%.3f, %.3f, %.3f, %.3f] (Euler [%.1f, %.1f, %.1f] deg) var: [%.1e, %.1e, %.1e]\n",
State::quat_nominal.idx, State::quat_nominal.idx + State::quat_nominal.dof - 1,
(double)_state.quat_nominal(0), (double)_state.quat_nominal(1), (double)_state.quat_nominal(2), (double)_state.quat_nominal(3),
(double)math::degrees(matrix::Eulerf(_state.quat_nominal).phi()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).theta()), (double)math::degrees(matrix::Eulerf(_state.quat_nominal).psi()),
(double)getStateVariance<State::quat_nominal>()(0), (double)getStateVariance<State::quat_nominal>()(1), (double)getStateVariance<State::quat_nominal>()(2)
);
printf("Velocity: [%.3f, %.3f, %.3f]\n",
(double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2));
printf("Velocity (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::vel.idx, State::vel.idx + State::vel.dof - 1,
(double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2),
(double)getStateVariance<State::vel>()(0), (double)getStateVariance<State::vel>()(1), (double)getStateVariance<State::vel>()(2)
);
printf("Position: [%.3f, %.3f, %.3f]\n",
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2));
printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::pos.idx, State::pos.idx + State::pos.dof - 1,
(double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2),
(double)getStateVariance<State::pos>()(0), (double)getStateVariance<State::pos>()(1), (double)getStateVariance<State::pos>()(2)
);
printf("Gyro Bias: [%.6f, %.6f, %.6f]\n",
(double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2));
printf("Gyro Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
State::gyro_bias.idx, State::gyro_bias.idx + State::gyro_bias.dof - 1,
(double)_state.gyro_bias(0), (double)_state.gyro_bias(1), (double)_state.gyro_bias(2),
(double)getStateVariance<State::gyro_bias>()(0), (double)getStateVariance<State::gyro_bias>()(1), (double)getStateVariance<State::gyro_bias>()(2)
);
printf("Accel Bias: [%.6f, %.6f, %.6f]\n",
(double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2));
printf("Accel Bias (%d-%d): [%.6f, %.6f, %.6f] var: [%.1e, %.1e, %.1e]\n",
State::accel_bias.idx, State::accel_bias.idx + State::accel_bias.dof - 1,
(double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_state.accel_bias(2),
(double)getStateVariance<State::accel_bias>()(0), (double)getStateVariance<State::accel_bias>()(1), (double)getStateVariance<State::accel_bias>()(2)
);
#if defined(CONFIG_EKF2_MAGNETOMETER)
printf("Magnetic Field: [%.3f, %.3f, %.3f]\n",
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2));
printf("Magnetic Field (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::mag_I.idx, State::mag_I.idx + State::mag_I.dof - 1,
(double)_state.mag_I(0), (double)_state.mag_I(1), (double)_state.mag_I(2),
(double)getStateVariance<State::mag_I>()(0), (double)getStateVariance<State::mag_I>()(1), (double)getStateVariance<State::mag_I>()(2)
);
printf("Magnetic Bias: [%.3f, %.3f, %.3f]\n",
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2));
printf("Magnetic Bias (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n",
State::mag_B.idx, State::mag_B.idx + State::mag_B.dof - 1,
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_state.mag_B(2),
(double)getStateVariance<State::mag_B>()(0), (double)getStateVariance<State::mag_B>()(1),
(double)getStateVariance<State::mag_B>()(2)
);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_WIND)
printf("Wind velocity: [%.3f, %.3f]\n", (double)_state.wind_vel(0), (double)_state.wind_vel(1));
printf("Wind velocity (%d-%d): [%.3f, %.3f] var: [%.1e, %.1e]\n",
State::wind_vel.idx, State::wind_vel.idx + State::wind_vel.dof - 1,
(double)_state.wind_vel(0), (double)_state.wind_vel(1),
(double)getStateVariance<State::wind_vel>()(0), (double)getStateVariance<State::wind_vel>()(1)
);
#endif // CONFIG_EKF2_WIND
printf("\nP:\n");