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

ekf2: verbose (DEBUG_BUILD) print status with Matrix improved print 

* ekf2: verbose print status
 * matrix/Matrix improve print output
 * bold diagonal elements, print ring buffer entry size
 * print in scientific notation when >= 10 to respect max size

---------

Co-authored-by: bresch <brescianimathieu@gmail.com>
This commit is contained in:
Daniel Agar 2024-01-10 09:46:04 -05:00 committed by GitHub
parent 071565a8ad
commit d45c3d3407
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
8 changed files with 178 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/lib/matrix/matrix/Matrix.hpp
View File

@ -365,13 +365,55 @@ public:
}
}
void print() const
void print(float eps = 0.00001f) const
{
// element: tab, point, 8 digits, 4 scientific notation chars; row: newline; string: \0 end
static const size_t n = 15 * N * M + M + 1;
char string[n];
write_string(string, n);
printf("%s\n", string);
// print column numbering
if (N > 1) {
printf(" ");
for (unsigned i = 0; i < N; i++) {
printf("|%2u ", i);
}
printf("\n");
}
const Matrix<Type, M, N> &self = *this;
for (unsigned i = 0; i < M; i++) {
printf("%2u|", i); // print row numbering
for (unsigned j = 0; j < N; j++) {
double d = static_cast<double>(self(i, j));
// Matrix diagonal elements
if (N > 1 && M > 1 && i == j) {
// make diagonal elements bold (ANSI CSI n 1)
printf("\033[1m");
}
// avoid -0.0 for display
if (fabs(d - 0.0) < (double)eps) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
} else {
printf("% 6.5f ", d);
}
// Matrix diagonal elements
if (N > 1 && M > 1 && i == j) {
// reset any formatting (ANSI CSI n 0)
printf("\033[0m");
}
}
printf("\n");
}
}
Matrix<Type, N, M> transpose() const

16
src/lib/matrix/test/MatrixAssignmentTest.cpp
View File

@ -276,14 +276,20 @@ TEST(MatrixAssignmentTest, Assignment)
}
}
char print_out[] = " | 0 | 1 \n 0| 1.00000 1.2e+04\n 1| 1.2e+04 0.12346\n 2| 1.2e+10 1.2e+12\n";
printf("%s\n", print_out); // for debugging in case of failure
// check print()
// Redirect stdout
EXPECT_TRUE(freopen("testoutput.txt", "w", stdout) != NULL);
FILE *fp = freopen("testoutput.txt", "w", stdout);
EXPECT_NE(fp, nullptr);
// write
Comma.print();
fclose(stdout);
EXPECT_FALSE(fclose(fp)); // FIXME: this doesn't work as expected, further printf are not redirected to the console
// read
FILE *fp = fopen("testoutput.txt", "r");
fp = fopen("testoutput.txt", "r");
EXPECT_NE(fp, nullptr);
EXPECT_FALSE(fseek(fp, 0, SEEK_SET));
@ -294,8 +300,8 @@ TEST(MatrixAssignmentTest, Assignment)
break;
}
printf("%d %d %d\n", static_cast<int>(i), output[i], c);
EXPECT_EQ(c, output[i]);
printf("%d %d %d\n", static_cast<int>(i), print_out[i], c);
EXPECT_EQ(c, print_out[i]);
}
EXPECT_FALSE(fclose(fp));

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

@ -153,6 +153,7 @@ public:
return false;
}
int get_used_size() const { return sizeof(*this) + sizeof(data_type) * entries(); }
int get_total_size() const { return sizeof(*this) + sizeof(data_type) * _size; }
int entries() const

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

@ -347,3 +347,96 @@ void Ekf::updateParameters()
_aux_global_position.updateParameters();
#endif // CONFIG_EKF2_AUX_GLOBAL_POSITION
}
template<typename T>
static void printRingBuffer(const char *name, RingBuffer<T> *rb)
{
if (rb) {
printf("%s: %d/%d entries (%d/%d Bytes) (%zu Bytes per entry)\n",
name,
rb->entries(), rb->get_length(), rb->get_used_size(), rb->get_total_size(),
sizeof(T));
}
}
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("Velocity: [%.3f, %.3f, %.3f]\n",
(double)_state.vel(0), (double)_state.vel(1), (double)_state.vel(2));
printf("Position: [%.3f, %.3f, %.3f]\n",
(double)_state.pos(0), (double)_state.pos(1), (double)_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("Accel Bias: [%.6f, %.6f, %.6f]\n",
(double)_state.accel_bias(0), (double)_state.accel_bias(1), (double)_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 Bias: [%.3f, %.3f, %.3f]\n",
(double)_state.mag_B(0), (double)_state.mag_B(1), (double)_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));
#endif // CONFIG_EKF2_WIND
printf("\nP:\n");
P.print();
printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
printf("minimum observation interval %d us\n", _min_obs_interval_us);
printRingBuffer("IMU buffer", &_imu_buffer);
printRingBuffer("system flag buffer", _system_flag_buffer);
#if defined(CONFIG_EKF2_AIRSPEED)
printRingBuffer("airspeed buffer", _airspeed_buffer);
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_AUXVEL)
printRingBuffer("aux vel buffer", _auxvel_buffer);
#endif // CONFIG_EKF2_AUXVEL
#if defined(CONFIG_EKF2_BAROMETER)
printRingBuffer("baro buffer", _baro_buffer);
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_DRAG_FUSION)
printRingBuffer("drag buffer", _drag_buffer);
#endif // CONFIG_EKF2_DRAG_FUSION
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
printRingBuffer("ext vision buffer", _ext_vision_buffer);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_GNSS)
printRingBuffer("gps buffer", _gps_buffer);
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
printRingBuffer("mag buffer", _mag_buffer);
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
printRingBuffer("flow buffer", _flow_buffer);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_RANGE_FINDER)
printRingBuffer("range buffer", _range_buffer);
#endif // CONFIG_EKF2_RANGE_FINDER
_output_predictor.print_status();
}

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

@ -85,6 +85,8 @@ public:
// initialise variables to sane values (also interface class)
bool init(uint64_t timestamp) override;
void print_status();
// should be called every time new data is pushed into the filter
bool update();

59
src/modules/ekf2/EKF/estimator_interface.cpp
View File

@ -676,62 +676,3 @@ void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name)
ECL_ERR("%s buffer allocation failed", buffer_name);
}
}
void EstimatorInterface::print_status()
{
printf("EKF average dt: %.6f seconds\n", (double)_dt_ekf_avg);
printf("IMU buffer: %d (%d Bytes)\n", _imu_buffer.get_length(), _imu_buffer.get_total_size());
printf("minimum observation interval %d us\n", _min_obs_interval_us);
#if defined(CONFIG_EKF2_GNSS)
if (_gps_buffer) {
printf("gps buffer: %d/%d (%d Bytes)\n", _gps_buffer->entries(), _gps_buffer->get_length(), _gps_buffer->get_total_size());
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_MAGNETOMETER)
if (_mag_buffer) {
printf("mag buffer: %d/%d (%d Bytes)\n", _mag_buffer->entries(), _mag_buffer->get_length(), _mag_buffer->get_total_size());
}
#endif // CONFIG_EKF2_MAGNETOMETER
#if defined(CONFIG_EKF2_BAROMETER)
if (_baro_buffer) {
printf("baro buffer: %d/%d (%d Bytes)\n", _baro_buffer->entries(), _baro_buffer->get_length(), _baro_buffer->get_total_size());
}
#endif // CONFIG_EKF2_BAROMETER
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_range_buffer) {
printf("range buffer: %d/%d (%d Bytes)\n", _range_buffer->entries(), _range_buffer->get_length(), _range_buffer->get_total_size());
}
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_AIRSPEED)
if (_airspeed_buffer) {
printf("airspeed buffer: %d/%d (%d Bytes)\n", _airspeed_buffer->entries(), _airspeed_buffer->get_length(), _airspeed_buffer->get_total_size());
}
#endif // CONFIG_EKF2_AIRSPEED
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_flow_buffer) {
printf("flow buffer: %d/%d (%d Bytes)\n", _flow_buffer->entries(), _flow_buffer->get_length(), _flow_buffer->get_total_size());
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_ext_vision_buffer) {
printf("vision buffer: %d/%d (%d Bytes)\n", _ext_vision_buffer->entries(), _ext_vision_buffer->get_length(), _ext_vision_buffer->get_total_size());
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_DRAG_FUSION)
if (_drag_buffer) {
printf("drag buffer: %d/%d (%d Bytes)\n", _drag_buffer->entries(), _drag_buffer->get_length(), _drag_buffer->get_total_size());
}
#endif // CONFIG_EKF2_DRAG_FUSION
_output_predictor.print_status();
}

6
src/modules/ekf2/EKF/estimator_interface.h
View File

@ -312,8 +312,6 @@ public:
const MapProjection &global_origin() const { return _pos_ref; }
float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; }
void print_status();
OutputPredictor &output_predictor() { return _output_predictor; };
protected:
@ -479,7 +477,7 @@ protected:
warning_event_status_u _warning_events{};
information_event_status_u _information_events{};
private:
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
#if defined(CONFIG_EKF2_DRAG_FUSION)
void setDragData(const imuSample &imu);
@ -492,7 +490,5 @@ private:
void printBufferAllocationFailed(const char *buffer_name);
ImuDownSampler _imu_down_sampler{_params.filter_update_interval_us};
unsigned _min_obs_interval_us{0}; // minimum time interval between observations that will guarantee data is not lost (usec)
};
#endif // !EKF_ESTIMATOR_INTERFACE_H

28
src/modules/ekf2/EKF/output_predictor.cpp
View File

@ -41,16 +41,32 @@ using matrix::Vector3f;
void OutputPredictor::print_status()
{
printf("output predictor: IMU dt: %.4f, EKF dt: %.4f\n", (double)_dt_update_states_avg, (double)_dt_correct_states_avg);
printf("[output predictor] IMU dt: %.6f, EKF dt: %.6f\n",
(double)_dt_update_states_avg, (double)_dt_correct_states_avg);
printf("output predictor: tracking error, angular: %.6f rad, velocity: %.3f m/s, position: %.3f m\n",
const matrix::Quatf q_att = _output_buffer.get_newest().quat_nominal;
const matrix::Eulerf euler = q_att;
printf("[output predictor] orientation: [%.4f, %.4f, %.4f, %.4f] (Euler [%.3f, %.3f, %.3f])\n",
(double)q_att(0), (double)q_att(1), (double)q_att(2), (double)q_att(3),
(double)euler.phi(), (double)euler.theta(), (double)euler.psi());
printf("[output predictor] velocity: [%.3f, %.3f, %.3f]\n",
(double)_output_buffer.get_newest().vel(0), (double)_output_buffer.get_newest().vel(1),
(double)_output_buffer.get_newest().vel(2));
printf("[output predictor] position: [%.3f, %.3f, %.3f]\n",
(double)_output_buffer.get_newest().pos(0), (double)_output_buffer.get_newest().pos(1),
(double)_output_buffer.get_newest().pos(2));
printf("[output predictor] tracking error, angular: %.6f rad, velocity: %.4f m/s, position: %.4f m\n",
(double)_output_tracking_error(0), (double)_output_tracking_error(1), (double)_output_tracking_error(2));
printf("output buffer: %d/%d (%d Bytes)\n", _output_buffer.entries(), _output_buffer.get_length(),
_output_buffer.get_total_size());
printf("[output predictor] output buffer: %d/%d (%d Bytes)\n",
_output_buffer.entries(), _output_buffer.get_length(), _output_buffer.get_total_size());
printf("output vert buffer: %d/%d (%d Bytes)\n", _output_vert_buffer.entries(), _output_vert_buffer.get_length(),
_output_vert_buffer.get_total_size());
printf("[output predictor] output vert buffer: %d/%d (%d Bytes)\n",
_output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size());
}
void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state)