EKF: support passing in an external position estimate

Tools/Replay/LR_MsgHandler.cpp

@@ -289,6 +289,12 @@ void LR_MsgHandler_REPH::process_message(uint8_t *msgbytes)
     AP::dal().handle_message(msg, ekf2, ekf3);
 }
 
+void LR_MsgHandler_RSLL::process_message(uint8_t *msgbytes)
+{
+    MSG_CREATE(RSLL, msgbytes);
+    AP::dal().handle_message(msg, ekf2, ekf3);
+}
+
 void LR_MsgHandler_REVH::process_message(uint8_t *msgbytes)
 {
     MSG_CREATE(REVH, msgbytes);

Tools/Replay/LR_MsgHandler.h

@@ -57,6 +57,12 @@ class LR_MsgHandler_REPH : public LR_MsgHandler_EKF
     void process_message(uint8_t *msg) override;
 };
 
+class LR_MsgHandler_RSLL : public LR_MsgHandler_EKF
+{
+    using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
+    void process_message(uint8_t *msg) override;
+};
+
 class LR_MsgHandler_REVH : public LR_MsgHandler_EKF
 {
     using LR_MsgHandler_EKF::LR_MsgHandler_EKF;

Tools/Replay/LogReader.cpp

@@ -118,6 +118,8 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
         msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
     } else if (streq(name, "REPH")) {
         msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3);
+	} else if (streq(name, "RSLL")) {
+        msgparser[f.type] = new LR_MsgHandler_RSLL(formats[f.type], ekf2, ekf3);
     } else if (streq(name, "REVH")) {
         msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3);
     } else if (streq(name, "RWOH")) {

Tools/autotest/arduplane.py

@@ -579,6 +579,95 @@ def DO_REPOSITION(self):
 
         self.fly_home_land_and_disarm()
 
+    def ExternalPositionEstimate(self):
+        '''Test mavlink EXTERNAL_POSITION_ESTIMATE command'''
+        if not hasattr(mavutil.mavlink, 'MAV_CMD_EXTERNAL_POSITION_ESTIMATE'):
+            return
+        self.change_mode("TAKEOFF")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.wait_altitude(48, 52, relative=True)
+
+        loc = self.mav.location()
+        self.location_offset_ne(loc, 2000, 2000)
+
+        # setting external position fail while we have GPS lock
+        self.progress("set new position with GPS")
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE,
+            self.get_sim_time()-1, # transmit time
+            0.5, # processing delay
+            50, # accuracy
+            0,
+            int(loc.lat * 1e7),
+            int(loc.lng * 1e7),
+            float("NaN"),    # alt
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
+            want_result=mavutil.mavlink.MAV_RESULT_FAILED,
+        )
+
+        self.progress("disable the GPS")
+        self.run_auxfunc(
+            65,
+            2,
+            want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
+        )
+
+        # fly for a bit to get into non-aiding state
+        self.progress("waiting 20 seconds")
+        tstart = self.get_sim_time()
+        while self.get_sim_time() < tstart + 20:
+            self.wait_heartbeat()
+
+        self.progress("getting base position")
+        gpi = self.mav.recv_match(
+            type='GLOBAL_POSITION_INT',
+            blocking=True,
+            timeout=5
+        )
+        loc = mavutil.location(gpi.lat*1e-7, gpi.lon*1e-7, 0, 0)
+
+        self.progress("set new position with no GPS")
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_EXTERNAL_POSITION_ESTIMATE,
+            self.get_sim_time()-1, # transmit time
+            0.5, # processing delay
+            50, # accuracy
+            0,
+            gpi.lat+1,
+            gpi.lon+1,
+            float("NaN"),    # alt
+            frame=mavutil.mavlink.MAV_FRAME_GLOBAL,
+            want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
+        )
+
+        self.progress("waiting 3 seconds")
+        tstart = self.get_sim_time()
+        while self.get_sim_time() < tstart + 3:
+            self.wait_heartbeat()
+
+        gpi2 = self.mav.recv_match(
+            type='GLOBAL_POSITION_INT',
+            blocking=True,
+            timeout=5
+        )
+        loc2 = mavutil.location(gpi2.lat*1e-7, gpi2.lon*1e-7, 0, 0)
+        dist = self.get_distance(loc, loc2)
+
+        self.progress("dist is %.1f" % dist)
+        if dist > 200:
+            raise NotAchievedException("Position error dist=%.1f" % dist)
+
+        self.progress("re-enable the GPS")
+        self.run_auxfunc(
+            65,
+            0,
+            want_result=mavutil.mavlink.MAV_RESULT_ACCEPTED
+        )
+
+        self.progress("flying home")
+        self.fly_home_land_and_disarm()
+
     def DeepStall(self):
         '''Test DeepStall Landing'''
         # self.fly_deepstall_absolute()
@@ -4603,6 +4692,7 @@ def tests(self):
             self.SDCardWPTest,
             self.NoArmWithoutMissionItems,
             self.MODE_SWITCH_RESET,
+            self.ExternalPositionEstimate,
         ])
         return ret
 

Tools/autotest/common.py

@@ -2977,7 +2977,7 @@ def LoggerDocumentation(self):
         REPLAY_MSGS = ['RFRH', 'RFRF', 'REV2', 'RSO2', 'RWA2', 'REV3', 'RSO3', 'RWA3', 'RMGI',
                        'REY3', 'RFRN', 'RISH', 'RISI', 'RISJ', 'RBRH', 'RBRI', 'RRNH', 'RRNI',
                        'RGPH', 'RGPI', 'RGPJ', 'RASH', 'RASI', 'RBCH', 'RBCI', 'RVOH', 'RMGH',
-                       'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH']
+                       'ROFH', 'REPH', 'REVH', 'RWOH', 'RBOH', 'RSLL']
 
         docco_ids = {}
         for thing in tree.logformat:

libraries/AP_AHRS/AP_AHRS.cpp

@@ -1287,6 +1287,16 @@ bool AP_AHRS::set_origin(const Location &loc)
     return false;
 }
 
+#if AP_AHRS_POSITION_RESET_ENABLED
+bool AP_AHRS::handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_ms)
+{
+#if HAL_NAVEKF3_AVAILABLE
+    return EKF3.setLatLng(loc, pos_accuracy, timestamp_ms);
+#endif
+    return false;
+}
+#endif
+
 // return true if inertial navigation is active
 bool AP_AHRS::have_inertial_nav(void) const
 {

libraries/AP_AHRS/AP_AHRS.h

@@ -194,6 +194,17 @@ class AP_AHRS {
     // from which to decide the origin on its own
     bool set_origin(const Location &loc) WARN_IF_UNUSED;
 
+#if AP_AHRS_POSITION_RESET_ENABLED
+    // Set the EKF's NE horizontal position states and their corresponding variances from the supplied WGS-84 location
+    // and 1-sigma horizontal position uncertainty. This can be used when the EKF is dead reckoning to periodically
+    // correct the position. If the EKF is is still using data from a postion sensor such as GPS, the position set
+    // will not be performed.
+    // pos_accuracy is the standard deviation of the horizontal position uncertainty in metres.
+    // The altitude element of the location is not used.
+    // Returns true if the set was successful.
+    bool handle_external_position_estimate(const Location &loc, float pos_accuracy, uint32_t timestamp_);
+#endif
+
     // returns the inertial navigation origin in lat/lon/alt
     bool get_origin(Location &ret) const WARN_IF_UNUSED;
 

libraries/AP_AHRS/AP_AHRS_config.h

@@ -14,3 +14,8 @@
 #ifndef AP_AHRS_SIM_ENABLED
 #define AP_AHRS_SIM_ENABLED AP_SIM_ENABLED
 #endif
+
+#ifndef AP_AHRS_POSITION_RESET_ENABLED
+#define AP_AHRS_POSITION_RESET_ENABLED (BOARD_FLASH_SIZE>1024)
+#endif
+

libraries/AP_DAL/AP_DAL.cpp

@@ -352,6 +352,17 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
     WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
 }
 
+void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
+{
+    end_frame();
+    const log_RSLL old = _RSLL;
+    _RSLL.lat = loc.lat;
+    _RSLL.lng = loc.lng;
+    _RSLL.posAccSD = posAccuracy;
+    _RSLL.timestamp_ms = timestamp_ms;
+    WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old);
+}
+
 // log external velocity data
 void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
 {
@@ -485,6 +496,17 @@ void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
     // note that EKF2 does not support body frame odomotry
     ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset);
 }
+
+/*
+  handle position reset
+ */
+void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
+{
+    _RSLL = msg;
+    // note that EKF2 does not support body frame odomotry
+    const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE };
+    ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms);
+}
 #endif // APM_BUILD_Replay
 
 namespace AP {

libraries/AP_DAL/AP_DAL.h

@@ -82,6 +82,8 @@ class AP_DAL {
 
     void log_event3(Event event);
     void log_SetOriginLLH3(const Location &loc);
+    void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
+
     void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);
     void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
 
@@ -317,6 +319,7 @@ class AP_DAL {
     void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
+    void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
 
     // map core number for replay
     uint8_t logging_core(uint8_t c) const;
@@ -340,6 +343,7 @@ class AP_DAL {
     struct log_REVH _REVH;
     struct log_RWOH _RWOH;
     struct log_RBOH _RBOH;
+    struct log_RSLL _RSLL;
 
     // cached variables for speed:
     uint32_t _micros;

libraries/AP_DAL/LogStructure.h

@@ -35,6 +35,7 @@
     LOG_RMGI_MSG, \
     LOG_ROFH_MSG, \
     LOG_REPH_MSG, \
+    LOG_RSLL_MSG, \
     LOG_REVH_MSG, \
     LOG_RWOH_MSG, \
     LOG_RBOH_MSG
@@ -324,6 +325,16 @@ struct log_REPH {
     uint8_t _end;
 };
 
+// @LoggerMessage: RSLL
+// @Description: Replay Set Lat Lng event
+struct log_RSLL {
+    int32_t lat; // WGS-84 latitude in 1E-7 degrees
+    int32_t lng; // WGS-84 longitude in 1E7 degrees
+    float posAccSD; // horizontal position 1 STD uncertainty (m)
+    uint32_t timestamp_ms;
+    uint8_t _end;
+};
+
 // @LoggerMessage: REVH
 // @Description: Replay external position data
 struct log_REVH {
@@ -417,6 +428,8 @@ struct log_RBOH {
       "ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \
       "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
+    { LOG_RSLL_MSG, RLOG_SIZE(RSLL),                         \
+      "RSLL", "IIfI", "Lat,Lng,PosAccSD,TS", "DU--", "GG--" }, \
     { LOG_REVH_MSG, RLOG_SIZE(REVH),                                   \
       "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \
     { LOG_RWOH_MSG, RLOG_SIZE(RWOH),                                   \

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1408,6 +1408,26 @@ bool NavEKF3::setOriginLLH(const Location &loc)
     return ret;
 }
 
+bool NavEKF3::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
+{
+#if EK3_FEATURE_POSITION_RESET
+    AP::dal().log_SetLatLng(loc, posAccuracy, timestamp_ms);
+
+    if (!core) {
+        return false;
+    }
+    bool ret = false;
+    for (uint8_t i=0; i<num_cores; i++) {
+        ret |= core[i].setLatLng(loc, posAccuracy, timestamp_ms);
+    }
+    // return true if any core accepts the new origin
+    return ret;
+#else
+    return false;
+#endif // EK3_FEATURE_POSITION_RESET
+}
+
+
 // return estimated height above ground level
 // return false if ground height is not being estimated.
 bool NavEKF3::getHAGL(float &HAGL) const

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -152,6 +152,11 @@ class NavEKF3 {
     // Returns false if the filter has rejected the attempt to set the origin
     bool setOriginLLH(const Location &loc);
 
+    // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty
+    // The altitude element of the location is not used.
+    // Returns true if the set was successful
+    bool setLatLng(const Location &loc, float posErr, uint32_t timestamp_ms);
+
     // return estimated height above ground level
     // return false if ground height is not being estimated.
     bool getHAGL(float &HAGL) const;

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -144,6 +144,42 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
     lastPosPassTime_ms = imuSampleTime_ms;
 }
 
+#if EK3_FEATURE_POSITION_RESET
+// Sets the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty
+// The altitude element of the location is not used.
+// Returns true if the set was successful
+bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
+{
+    if ((imuSampleTime_ms - lastPosPassTime_ms) < frontend->deadReckonDeclare_ms ||
+        (PV_AidingMode == AID_NONE)
+        || !validOrigin) {
+        return false;
+    }
+
+    // Store the position before the reset so that we can record the reset delta
+    posResetNE.x = stateStruct.position.x;
+    posResetNE.y = stateStruct.position.y;
+
+    // reset the corresponding covariances
+    zeroRows(P,7,8);
+    zeroCols(P,7,8);
+
+    // set the variances using the position measurement noise parameter
+    P[7][7] = P[8][8] = sq(MAX(posAccuracy,frontend->_gpsHorizPosNoise));
+
+    // Correct the position for time delay relative to fusion time horizon assuming a constant velocity
+    // Limit time stamp to a range between current time and 5 seconds ago
+    const uint32_t timeStampConstrained_ms = MAX(MIN(timestamp_ms, imuSampleTime_ms), imuSampleTime_ms - 5000);
+    const int32_t delta_ms = int32_t(imuDataDelayed.time_ms - timeStampConstrained_ms);
+    const ftype delaySec = 1E-3F * ftype(delta_ms);
+    const Vector2F newPosNE = EKF_origin.get_distance_NE_ftype(loc) - stateStruct.velocity.xy() * delaySec;
+    ResetPositionNE(newPosNE.x,newPosNE.y);
+
+    return true;
+}
+#endif // EK3_FEATURE_POSITION_RESET
+
+
 // reset the stateStruct's NE position to the specified position
 //    posResetNE is updated to hold the change in position
 //    storedOutput, outputDataNew and outputDataDelayed are updated with the change in position

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -230,6 +230,11 @@ class NavEKF3_core : public NavEKF_core_common
     // returns false if Absolute aiding and GPS is being used or if the origin is already set
     bool setOriginLLH(const Location &loc);
 
+    // Set the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty
+    // The altitude element of the location is not used.
+    // Returns true if the set was successful
+    bool setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
+
     // return estimated height above ground level
     // return false if ground height is not being estimated.
     bool getHAGL(float &HAGL) const;

libraries/AP_NavEKF3/AP_NavEKF3_feature.h

@@ -7,6 +7,7 @@
 #include <AP_Vehicle/AP_Vehicle_Type.h>
 #include <AP_HAL/AP_HAL_Boards.h>
 #include <AP_Beacon/AP_Beacon_config.h>
+#include <AP_AHRS/AP_AHRS_config.h>
 
 // define for when to include all features
 #define EK3_FEATURE_ALL APM_BUILD_TYPE(APM_BUILD_AP_DAL_Standalone) || APM_BUILD_TYPE(APM_BUILD_Replay)
@@ -30,3 +31,7 @@
 #ifndef EK3_FEATURE_BEACON_FUSION
 #define EK3_FEATURE_BEACON_FUSION AP_BEACON_ENABLED
 #endif
+
+#ifndef EK3_FEATURE_POSITION_RESET
+#define EK3_FEATURE_POSITION_RESET EK3_FEATURE_ALL || AP_AHRS_POSITION_RESET_ENABLED
+#endif