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_replay: Display RMS innovation values to assist with tuning

Browse Source 
Displaying the RMS innovation values at the end of each replay assets with rapid iteration for time delay parameter tuning without having to plot or post process using another tool.
This commit is contained in:
Paul Riseborough 2016-11-08 22:18:12 +11:00 committed by Lorenz Meier
parent ab76a37910
commit bdec646736
1 changed files with 33 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/ekf2_replay/ekf2_replay_main.cpp
View File

@ -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[])