diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 92e5303a6a..ccbd2386db 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -4,3 +4,4 @@ float32 n_states # Number of states effectively used uint8 nan_flags # Bitmask to indicate NaN states uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) +float32[28] covariances # Diagonal Elements of Covariance Matrix diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 7084f716c0..0e991a51a5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -66,6 +66,8 @@ #include #include +#include + #include #include @@ -258,6 +260,11 @@ private: AttPosEKF *_ekf; + /* Low pass filter for attitude rates */ + math::LowPassFilter2p _LP_att_P; + math::LowPassFilter2p _LP_att_Q; + math::LowPassFilter2p _LP_att_R; + private: /** * Update our local parameter cache. diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 3b447068c8..79b7afe5c7 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -200,9 +200,13 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _newRangeData(false), _mavlink_fd(-1), - _parameters {}, - _parameter_handles {}, - _ekf(nullptr) + _parameters{}, + _parameter_handles{}, + _ekf(nullptr), + + _LP_att_P(100.0f, 10.0f), + _LP_att_Q(100.0f, 10.0f), + _LP_att_R(100.0f, 10.0f) { _last_run = hrt_absolute_time(); @@ -474,8 +478,13 @@ int AttitudePositionEstimatorEKF::check_filter_state() size_t max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + // Copy diagonal elemnts of covariance matrix + float covariances[28]; + _ekf->get_covariance(covariances); + for (size_t i = 0; i < rep.n_states; i++) { rep.states[i] = ekf_report.states[i]; + rep.covariances[i] = covariances[i]; } @@ -819,9 +828,9 @@ void AttitudePositionEstimatorEKF::publishAttitude() _att.pitch = euler(1); _att.yaw = euler(2); - _att.rollspeed = _ekf->angRate.x - _ekf->states[10] / _ekf->dtIMUfilt; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11] / _ekf->dtIMUfilt; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12] / _ekf->dtIMUfilt; + _att.rollspeed = _LP_att_P.apply(_ekf->angRate.x) - _ekf->states[10] / _ekf->dtIMUfilt; + _att.pitchspeed = _LP_att_Q.apply(_ekf->angRate.y) - _ekf->states[11] / _ekf->dtIMUfilt; + _att.yawspeed = _LP_att_R.apply(_ekf->angRate.z) - _ekf->states[12] / _ekf->dtIMUfilt; // gyro offsets _att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt; diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 5d56dbaae3..bf70ed13bc 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -3337,3 +3337,10 @@ void AttPosEKF::setIsFixedWing(const bool fixedWing) { _isFixedWing = fixedWing; } + +void AttPosEKF::get_covariance(float c[EKF_STATE_ESTIMATES]) +{ + for (unsigned int i = 0; i < EKF_STATE_ESTIMATES; i++) { + c[i] = P[i][i]; + } +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.h b/src/modules/ekf_att_pos_estimator/estimator_22states.h index 9b23f4df44..426340d2ce 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.h @@ -379,6 +379,8 @@ public: */ void ZeroVariables(); + void get_covariance(float c[28]); + protected: /** diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8548b90b56..ff8546bf94 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,6 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; + struct log_EST2_s log_EST2; + struct log_EST3_s log_EST3; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; struct log_VISN_s log_VISN; @@ -1845,6 +1847,18 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); LOGBUFFER_WRITE_AND_COUNT(EST1); + + log_msg.msg_type = LOG_EST2_MSG; + unsigned maxcopy2 = (sizeof(buf.estimator_status.covariances) < sizeof(log_msg.body.log_EST2.cov)) ? sizeof(buf.estimator_status.covariances) : sizeof(log_msg.body.log_EST2.cov); + memset(&(log_msg.body.log_EST2.cov), 0, sizeof(log_msg.body.log_EST2.cov)); + memcpy(&(log_msg.body.log_EST2.cov), buf.estimator_status.covariances, maxcopy2); + LOGBUFFER_WRITE_AND_COUNT(EST2); + + log_msg.msg_type = LOG_EST3_MSG; + unsigned maxcopy3 = ((sizeof(buf.estimator_status.covariances) - maxcopy2) < sizeof(log_msg.body.log_EST3.cov)) ? (sizeof(buf.estimator_status.covariances) - maxcopy2) : sizeof(log_msg.body.log_EST3.cov); + memset(&(log_msg.body.log_EST3.cov), 0, sizeof(log_msg.body.log_EST3.cov)); + memcpy(&(log_msg.body.log_EST3.cov), buf.estimator_status.covariances + maxcopy2, maxcopy3); + LOGBUFFER_WRITE_AND_COUNT(EST3); } /* --- TECS STATUS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 9cf37683ae..0929532e42 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -402,11 +402,23 @@ struct log_EST1_s { float s[16]; }; +/* --- EST2 - ESTIMATOR STATUS --- */ +#define LOG_EST2_MSG 34 +struct log_EST2_s { + float cov[12]; +}; + +/* --- EST3 - ESTIMATOR STATUS --- */ +#define LOG_EST3_MSG 35 +struct log_EST3_s { + float cov[16]; +}; + /* --- TEL0..3 - TELEMETRY STATUS --- */ -#define LOG_TEL0_MSG 34 -#define LOG_TEL1_MSG 35 -#define LOG_TEL2_MSG 36 -#define LOG_TEL3_MSG 37 +#define LOG_TEL0_MSG 36 +#define LOG_TEL1_MSG 37 +#define LOG_TEL2_MSG 38 +#define LOG_TEL3_MSG 39 struct log_TEL_s { uint8_t rssi; uint8_t remote_rssi; @@ -419,7 +431,7 @@ struct log_TEL_s { }; /* --- VISN - VISION POSITION --- */ -#define LOG_VISN_MSG 38 +#define LOG_VISN_MSG 40 struct log_VISN_s { float x; float y; @@ -434,7 +446,7 @@ struct log_VISN_s { }; /* --- ENCODERS - ENCODER DATA --- */ -#define LOG_ENCD_MSG 39 +#define LOG_ENCD_MSG 41 struct log_ENCD_s { int64_t cnt0; float vel0; @@ -443,22 +455,22 @@ struct log_ENCD_s { }; /* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ -#define LOG_AIR1_MSG 41 +#define LOG_AIR1_MSG 42 /* --- VTOL - VTOL VEHICLE STATUS */ -#define LOG_VTOL_MSG 42 +#define LOG_VTOL_MSG 43 struct log_VTOL_s { float airspeed_tot; }; /* --- TIMESYNC - TIME SYNCHRONISATION OFFSET */ -#define LOG_TSYN_MSG 43 +#define LOG_TSYN_MSG 44 struct log_TSYN_s { uint64_t time_offset; }; /* --- MACS - MULTIROTOR ATTITUDE CONTROLLER STATUS */ -#define LOG_MACS_MSG 44 +#define LOG_MACS_MSG 45 struct log_MACS_s { float roll_rate_integ; float pitch_rate_integ; @@ -522,6 +534,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(TEL3, TEL, "BBBBHHBQ", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), + LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"), + LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"),