Add XKF (third Kalman filter) analyzer

Author: peterbarker

LA_MsgHandler.cpp

@@ -181,7 +181,7 @@ void LA_MsgHandler_ATT::xprocess(const uint8_t *msg) {
 }
 
 
-// TODO: if a third Kalman filter exists, factor this EKF1 and NKF1
+// TODO: factor with NKF1 and XKF1
 LA_MsgHandler_EKF1::LA_MsgHandler_EKF1(std::string name,
                                        const struct log_Format &f,
                                        Analyze *analyze,
@@ -508,6 +508,78 @@ void LA_MsgHandler_NKF1::xprocess(const uint8_t *msg) {
 
 }
 
+// FIXME: factor!
+LA_MsgHandler_XKF1::LA_MsgHandler_XKF1(std::string name,
+                                       const struct log_Format &f,
+                                       Analyze *analyze,
+                                       AnalyzerVehicle::Base *&vehicle) :
+    LA_MsgHandler(name, f, analyze, vehicle) {
+    _analyze->add_data_source("ATTITUDE_ESTIMATE_XKF1", "XKF1.Roll");
+    _analyze->add_data_source("ATTITUDE_ESTIMATE_XKF1", "XKF1.Pitch");
+    _analyze->add_data_source("ATTITUDE_ESTIMATE_XKF1", "XKF1.Yaw");
+
+    _analyze->add_data_source("POSITION_ESTIMATE_XKF1", "XKF1.PN");
+    _analyze->add_data_source("POSITION_ESTIMATE_XKF1", "XKF1.PE");
+
+    _analyze->add_data_source("ALTITUDE_ESTIMATE_XKF1", "XKF1.PD");
+
+    _analyze->add_data_source("VELOCITY_ESTIMATE_XKF1", "XKF1.VN");
+    _analyze->add_data_source("VELOCITY_ESTIMATE_XKF1", "XKF1.VE");
+    _analyze->add_data_source("VELOCITY_ESTIMATE_XKF1", "XKF1.VD");
+
+}
+
+void LA_MsgHandler_XKF1::xprocess(const uint8_t *msg) {
+    int16_t Roll = require_field_int16_t(msg, "Roll");
+    int16_t Pitch = require_field_int16_t(msg, "Pitch");
+    float Yaw = require_field_float(msg, "Yaw");
+
+    _vehicle->attitude_estimate("XKF1")->set_roll(T(), Roll/(double)100.0f);
+    _vehicle->attitude_estimate("XKF1")->set_pitch(T(), Pitch/(double)100.0f);
+    _vehicle->attitude_estimate("XKF1")->set_yaw(T(), Yaw-180);
+
+    // these are all relative; need to work out an origin:
+    if (_vehicle->origin_lat_T() != 0) {
+        double posN = require_field_float(msg, "PN");
+        double posE = require_field_float(msg, "PE");
+        double origin_lat = _vehicle->origin_lat();
+        double origin_lon = _vehicle->origin_lon();
+
+        double lat = 0;
+        double lon = 0;
+        gps_offset(origin_lat, origin_lon, posE, posN, lat, lon);
+        // ::fprintf(stderr, "%f+%f / %f+%f = %f / %f\n",
+        //           origin_lat, posE, origin_lon, posN, lat, lon);
+
+        _vehicle->position_estimate("XKF1")->set_lat(T(), lat);
+        _vehicle->position_estimate("XKF1")->set_lon(T(), lon);
+    }
+    if (_vehicle->origin_altitude_T() != 0) {
+        double posD = require_field_float(msg, "PD");
+        double origin_alt = _vehicle->origin_altitude();
+        _vehicle->altitude_estimate("XKF1")->set_alt(T(), origin_alt - posD);
+    }
+
+    {
+        double vn = require_field_float(msg, "VN");
+        double ve = require_field_float(msg, "VE");
+        double vd = require_field_float(msg, "VD");
+
+        _vehicle->velocity_estimate("XKF1")->velocity().set_x(T(), vn);
+        _vehicle->velocity_estimate("XKF1")->velocity().set_y(T(), ve);
+        _vehicle->velocity_estimate("XKF1")->velocity().set_z(T(), vd);
+
+        if (int(_vehicle->require_param_with_defaults("AHRS_EKF_TYPE")) == 2) {
+            // set XKF1 as canonical for velocity:
+            // Sadly, NTUN isn't updated if we're not using the nav controller
+            _vehicle->vel().set_x(T(), vn);
+            _vehicle->vel().set_y(T(), ve);
+            _vehicle->vel().set_z(T(), vd);
+        }
+    }
+
+}
+
 void LA_MsgHandler_PM::xprocess(const uint8_t *msg)
 {
     uint16_t nlon;

LA_MsgHandler.h

@@ -336,6 +336,39 @@ class LA_MsgHandler_NKF4 : public LA_MsgHandler {
     }
 };
 
+class LA_MsgHandler_XKF1 : public LA_MsgHandler {
+public:
+    LA_MsgHandler_XKF1(std::string name, const struct log_Format &f,
+                       Analyze *analyze, AnalyzerVehicle::Base *&vehicle);
+    void xprocess(const uint8_t *msg) override;
+};
+
+class LA_MsgHandler_XKF4 : public LA_MsgHandler {
+public:
+    LA_MsgHandler_XKF4(std::string name, const struct log_Format &f, Analyze *analyze, AnalyzerVehicle::Base *&vehicle) :
+        LA_MsgHandler(name, f, analyze, vehicle) {
+        _analyze->add_data_source("XKF_FLAGS", "XKF4.SS");
+        _analyze->add_data_source("XKF_VARIANCES_velocity_variance", "XKF4.SV");
+        _analyze->add_data_source("XKF_VARIANCES_pos_horiz_variance", "XKF4.SP");
+        _analyze->add_data_source("XKF_VARIANCES_pos_vert_variance", "XKF4.SH");
+        _analyze->add_data_source("XKF_VARIANCES_pos_compass_variance", "XKF4.SM");
+        _analyze->add_data_source("XKF_VARIANCES_terrain_alt_variance", "XKF4.SVT");
+    };
+    void xprocess(const uint8_t *msg) override {
+        _vehicle->xkf_set_variance("velocity", require_field_uint16_t(msg, "SV") / (double)100.0f);
+        _vehicle->xkf_set_variance("pos_horiz", require_field_uint16_t(msg, "SP") / (double)100.0f);
+        _vehicle->xkf_set_variance("pos_vert", require_field_uint16_t(msg, "SH") / (double)100.0f);
+        _vehicle->xkf_set_variance("compass", require_field_uint16_t(msg, "SM") / (double)100.0f);
+        _vehicle->xkf_set_variance("terrain_alt", require_field_uint16_t(msg, "SVT") / (double)100.0f);
+
+        // FIXME: this will always be present
+        uint16_t ss;
+        if (field_value(msg, "SS", ss)) {
+            _vehicle->xkf_set_flags(ss);
+        }
+    }
+};
+
 class LA_MsgHandler_ORGN : public LA_MsgHandler {
 public:
     LA_MsgHandler_ORGN(std::string name, const struct log_Format &f, Analyze *analyze, AnalyzerVehicle::Base *&vehicle) :

analyze.cpp

@@ -16,6 +16,7 @@
 #include "analyzer/analyzer_ever_armed.h"
 #include "analyzer/analyzer_good_ekf.h"
 #include "analyzer/analyzer_good_nkf.h"
+#include "analyzer/analyzer_good_xkf.h"
 #include "analyzer/analyzer_gps_fix.h"
 #include "analyzer/analyzer_gyro_drift.h"
 #include "analyzer/analyzer_notcrashed.h"
@@ -133,6 +134,13 @@ void Analyze::instantiate_analyzers(INIReader *config)
         la_log(LOG_INFO, "Failed to create analyzer_good_nkf");
     }
 
+    Analyzer_Good_XKF *analyzer_good_xkf = new Analyzer_Good_XKF(vehicle,_data_sources);
+    if (analyzer_good_xkf != NULL) {
+        configure_analyzer(config, analyzer_good_xkf);
+    } else {
+        la_log(LOG_INFO, "Failed to create analyzer_good_xkf");
+    }
+
     Analyzer_GPS_Fix *analyzer_gps_fix = new Analyzer_GPS_Fix(vehicle,_data_sources);
     if (analyzer_gps_fix != NULL) {
         configure_analyzer(config, analyzer_gps_fix);

analyzer/analyzer_good_kf.cpp

@@ -177,7 +177,7 @@ void Analyzer_Good_KF::open_result_flags(uint16_t flags)
     _result_flags->set_reason(string_format("The %s status report indicates a problem with the EKF", shortname().c_str()));
     _result_flags->set_flags(this->flags());
 
-    _result_flags->add_evidence(string_format("flags=%d", flags));
+    _result_flags->add_evidence(string_format("flags=%u", flags));
     // TODO: put the flags and the "bad" descrption into a structure
     _result_flags->increase_severity_score(10);
     if (!(flags & EKF_ATTITUDE)) {
@@ -220,6 +220,10 @@ void Analyzer_Good_KF::open_result_flags(uint16_t flags)
         _result_flags->increase_severity_score(1);
         _result_flags->add_evidence("Predicted horizontal position (absolute) bad");
     }
+    if (flags & (1<<15)) {
+        _result_flags->increase_severity_score(1);
+        _result_flags->add_evidence("GPS is glitching");
+    }
 
     if (_vehicle->any_acc_clipping()) {
         _result_flags->add_evidence("Vehicle Accelerometers Clipping");

analyzer/analyzer_good_xkf.cpp

@@ -0,0 +1,48 @@
+#ifndef ANALYZER_GOOD_XKF_H
+#define ANALYZER_GOOD_XKF_H
+
+/*
+ * analyzer_good_xkf
+ *
+ */
+
+#include "analyzer.h"
+#include "data_sources.h"
+#include "analyzer_good_kf.h"
+
+class Analyzer_Good_XKF : public Analyzer_Good_KF {
+
+public:
+
+    // borrow the constructor:
+    using Analyzer_Good_KF::Analyzer_Good_KF;
+
+    const std::string name() const override { return "Good EKF3"; }
+    const std::string description() const override {
+        return "ArduPilot's third Extended Kalman Filter (EKF3) has many built-in checks to ensure that it is functioning correctly.  This test will FAIL or WARN if EKF variances exceed the respective thresholds, or FAIL if the EKF status flags indicate errors.";
+    }
+
+    const std::string shortname_lc() const override { return "xkf"; }
+    const std::string shortname() const override { return "XKF"; }
+
+    uint64_t variance_T(std::string name) const override {
+        return _vehicle->xkf_variance_T(name);
+    }
+    uint64_t flags_T() const override {
+        return _vehicle->xkf_flags_T();
+    }
+    uint16_t flags() const override {
+        return _vehicle->xkf_flags();
+    }
+
+    std::map<const std::string, double> variances() {
+        return _vehicle->xkf_variances();
+    }
+
+private:
+
+};
+
+#endif
+
+

analyzer/analyzer_good_xkf.h

@@ -273,12 +273,18 @@ namespace AnalyzerVehicle {
         uint64_t _flags_T = 0;
     };
 
-    /// @brief State information for a base Extended Kalman Filter.
+    /// @brief State information for a second-edition Extended Kalman Filter.
     class NKF : public EKF {
     public:
     private:
     };
 
+    /// @brief State information for a third-edition Extended Kalman Filter.
+    class XKF : public NKF {
+    public:
+    private:
+    };
+
     // class AV_PosNED {
     // public:
     //     uint16_t N;
@@ -682,6 +688,38 @@ class Base {
         return _nkf.variances();
     }
 
+
+    /// @brief Set a variance value by name.
+    /// @param name Name of variance to set (e.g. "pos_horiz").
+    /// @param value New value of the variance.
+    void xkf_set_variance(const char *name, double value) {
+        _xkf.set_variance(T(), name, value);
+    }
+    uint64_t xkf_variance_T(std::string name) const {
+        return _xkf.variance_T(name);
+    }
+    /// @brief Set XKF (EKF3) status flags.
+    /// @param flags XKF (EKF3 self-assessment status flags.
+    void xkf_set_flags(uint16_t flags) {
+        _xkf.set_flags(T(), flags);
+    }
+    /// @brief XKF status flags.
+    /// @return XKF self-assessment status flags.
+    uint16_t xkf_flags() const {
+        return _xkf.flags();
+    }
+
+    /// @brief XKF (EKF3) status flags modification time.
+    /// @return timestamp Timestamp at which the XKF self-assessment flags were last changed (microseconds).
+    uint64_t xkf_flags_T() const {
+        return _xkf.flags_T();
+    }
+    /// @brief All XKF (EKF3) variances.
+    /// @return Returns all XKF (EKF3) variances as a map from name to value.
+    std::map<const std::string, double> xkf_variances() {
+        return _xkf.variances();
+    }
+
     /// @brief Retrieve parameter value.
     /// @param name Parameter value to retrieve.
     /// @detail This function will abort if the parameter has not been seen and no default is known.
@@ -1234,6 +1272,7 @@ class Base {
 
     EKF _ekf;
     NKF _nkf;
+    XKF _xkf;
 
     bool _armed = false;
 

analyzervehicle.h

@@ -51,6 +51,10 @@ void Analyzing_DataFlash_Message_Handler::handle_format_message_received(const c
         handlers[new_msg_type] = new LA_MsgHandler_NKF1(name, format, _analyze, _vehicle);
     } else if (streq(name, "NKF4")) {
         handlers[new_msg_type] = new LA_MsgHandler_NKF4(name, format, _analyze, _vehicle);
+    } else if (streq(name, "XKF1")) {
+        handlers[new_msg_type] = new LA_MsgHandler_XKF1(name, format, _analyze, _vehicle);
+    } else if (streq(name, "XKF4")) {
+        handlers[new_msg_type] = new LA_MsgHandler_XKF4(name, format, _analyze, _vehicle);
     } else if (streq(name, "ORGN")) {
         have_orgn = true;
         handlers[new_msg_type] = new LA_MsgHandler_ORGN(name, format, _analyze, _vehicle);