diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index 583b0d5548..62013b47ee 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -157,6 +157,13 @@ private: struct vision_position_estimate_s _ev; struct vehicle_status_s _vehicle_status; + uint32_t _numInnovSamples; // number of samples used to calculate the RMS innovation values + float _velInnovSumSq; // GPS velocity innovation sum of squares + float _posInnovSumSq; // GPS position innovation sum of squares + float _hgtInnovSumSq; // Vertical position innovation sum of squares + float _magInnovSumSq; // magnetometer innovation sum of squares + float _tasInnovSumSq; // airspeed innovation sum of squares + unsigned _message_counter; // counter which will increase with every message read from the log unsigned _part1_counter_ref; // this is the value of _message_counter when the part1 of the replay message is read (imu data) bool _read_part2; // indicates if part 2 of replay message has been read @@ -227,6 +234,12 @@ Ekf2Replay::Ekf2Replay(char *logfile) : _range{}, _airspeed{}, _vehicle_status{}, + _numInnovSamples(0), + _velInnovSumSq(0.0f), + _posInnovSumSq(0.0f), + _hgtInnovSumSq(0.0f), + _magInnovSumSq(0.0f), + _tasInnovSumSq(0.0f), _message_counter(0), _part1_counter_ref(0), _read_part2(false), @@ -697,6 +710,16 @@ void Ekf2Replay::logIfUpdated() log_message.body.innov3.s[4] = innov.hagl_innov; log_message.body.innov3.s[5] = innov.hagl_innov_var; writeMessage(_write_fd, (void *)&log_message.head1, _formats[LOG_EST6_MSG].length); + + // Update tuning metrics + _numInnovSamples++; + _velInnovSumSq += innov.vel_pos_innov[0] * innov.vel_pos_innov[0] + innov.vel_pos_innov[1] * innov.vel_pos_innov[1]; + _posInnovSumSq += innov.vel_pos_innov[3] * innov.vel_pos_innov[3] + innov.vel_pos_innov[4] * innov.vel_pos_innov[4]; + _hgtInnovSumSq += innov.vel_pos_innov[5] * innov.vel_pos_innov[5]; + _magInnovSumSq += innov.mag_innov[0] * innov.mag_innov[0] + innov.mag_innov[1] * innov.mag_innov[1] + innov.mag_innov[2] + * innov.mag_innov[2]; + _tasInnovSumSq += innov.airspeed_innov * innov.airspeed_innov; + } // update control state @@ -1005,6 +1028,16 @@ void Ekf2Replay::task_main() ::close(fd); delete ekf2_replay::instance; ekf2_replay::instance = nullptr; + + // Report sensor innovation RMS values to assist with time delay tuning + if (_numInnovSamples > 0) { + PX4_INFO("GPS vel innov RMS = %6.3f", sqrt(_velInnovSumSq / _numInnovSamples)); + PX4_INFO("GPS pos innov RMS = %6.3f", sqrt(_posInnovSumSq / _numInnovSamples)); + PX4_INFO("Hgt innov RMS = %6.3f", sqrt(_hgtInnovSumSq / _numInnovSamples)); + PX4_INFO("Mag innov RMS = %6.4f", sqrt(_magInnovSumSq / _numInnovSamples)); + PX4_INFO("TAS innov RMS = %6.3f", sqrt(_tasInnovSumSq / _numInnovSamples)); + } + } void Ekf2Replay::task_main_trampoline(int argc, char *argv[])