AC_PrecLand: add ekf to estimate vehicle-relative target velocity

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -22,6 +22,33 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("TYPE",    1, AC_PrecLand, _type, 0),
 
+    // @Param: YAW_ALIGN
+    // @DisplayName: Sensor yaw alignment
+    // @Description: Yaw angle from body x-axis to sensor x-axis.
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Advanced
+    // @Units: Centi-degrees
+    AP_GROUPINFO("YAW_ALIGN",    2, AC_PrecLand, _yaw_align, 0),
+
+    // @Param: LAND_OFS_X
+    // @DisplayName: Land offset forward
+    // @Description: Desired landing position of the camera forward of the target in vehicle body frame
+    // @Range: -20 20
+    // @Increment: 1
+    // @User: Advanced
+    // @Units: Centimeters
+    AP_GROUPINFO("LAND_OFS_X",    3, AC_PrecLand, _land_ofs_cm_x, 0),
+
+    // @Param: LAND_OFS_Y
+    // @DisplayName: Land offset right
+    // @Description: desired landing position of the camera right of the target in vehicle body frame
+    // @Range: -20 20
+    // @Increment: 1
+    // @User: Advanced
+    // @Units: Centimeters
+    AP_GROUPINFO("LAND_OFS_Y",    4, AC_PrecLand, _land_ofs_cm_y, 0),
+
     AP_GROUPEND
 };
 
@@ -81,13 +108,34 @@ void AC_PrecLand::init()
 }
 
 // update - give chance to driver to get updates from sensor
-void AC_PrecLand::update(float alt_above_terrain_cm)
+void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
 {
+    _attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
+    
     // run backend update
     if (_backend != NULL && _enabled) {
         // read from sensor
         _backend->update();
 
+        Vector3f vehicleVelocityNED = _inav.get_velocity()*0.01f;
+        vehicleVelocityNED.z = -vehicleVelocityNED.z;
+
+        if (target_acquired()) {
+            // EKF prediction step
+            float dt;
+            Vector3f targetDelVel;
+            _ahrs.getCorrectedDeltaVelocityNED(targetDelVel, dt);
+            targetDelVel = -targetDelVel;
+
+            _ekf_x.predict(dt, targetDelVel.x, 0.5f*dt);
+            _ekf_y.predict(dt, targetDelVel.y, 0.5f*dt);
+
+            if (_inav.get_filter_status().flags.horiz_pos_rel) {
+                _ekf_x.fuseVel(-vehicleVelocityNED.x, sq(1.0f));
+                _ekf_y.fuseVel(-vehicleVelocityNED.y, sq(1.0f));
+            }
+        }
+
         if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
             // we have a new, unique los measurement
             _last_backend_los_meas_ms = _backend->los_meas_time_ms();
@@ -95,67 +143,91 @@ void AC_PrecLand::update(float alt_above_terrain_cm)
             Vector3f target_vec_unit_body;
             _backend->get_los_body(target_vec_unit_body);
 
-            calc_angles_and_pos(target_vec_unit_body, alt_above_terrain_cm);
+            // Apply sensor yaw alignment rotation
+            float sin_yaw_align = sinf(radians(_yaw_align*0.01f));
+            float cos_yaw_align = cosf(radians(_yaw_align*0.01f));
+            Matrix3f Rz = Matrix3f(
+                cos_yaw_align, -sin_yaw_align, 0,
+                sin_yaw_align, cos_yaw_align, 0,
+                0, 0, 1
+            );
+
+            Vector3f target_vec_unit_ned = _attitude_history.front() * Rz * target_vec_unit_body;
+
+            bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
+
+            if (target_vec_valid && rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) {
+                float alt = MAX(rangefinder_alt_cm*0.01f, 0.0f);
+                float dist = alt/target_vec_unit_ned.z;
+                Vector3f targetPosRelMeasNED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
+
+                float xy_pos_var = sq(targetPosRelMeasNED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
+                if (!target_acquired()) {
+                    // reset filter state
+                    if (_inav.get_filter_status().flags.horiz_pos_rel) {
+                        _ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, -vehicleVelocityNED.x, sq(1.0f));
+                        _ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(1.0f));
+                    } else {
+                        _ekf_x.init(targetPosRelMeasNED.x, xy_pos_var, 0.0f, sq(10.0f));
+                        _ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, 0.0f, sq(10.0f));
+                    }
+                    _last_update_ms = AP_HAL::millis();
+                } else {
+                    float NIS_x = _ekf_x.getPosNIS(targetPosRelMeasNED.x, xy_pos_var);
+                    float NIS_y = _ekf_y.getPosNIS(targetPosRelMeasNED.y, xy_pos_var);
+                    if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
+                        _outlier_reject_count = 0;
+                        _ekf_x.fusePos(targetPosRelMeasNED.x, xy_pos_var);
+                        _ekf_y.fusePos(targetPosRelMeasNED.y, xy_pos_var);
+                        _last_update_ms = AP_HAL::millis();
+                    } else {
+                        _outlier_reject_count++;
+                    }
+                }
+            }
         }
     }
 }
 
-bool AC_PrecLand::target_acquired()
+bool AC_PrecLand::target_acquired() const
 {
-    return (AP_HAL::millis()-_last_update_ms) < 1000;
+    return (AP_HAL::millis()-_last_update_ms) < 2000;
 }
 
-bool AC_PrecLand::get_target_position(Vector3f& ret)
+bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const
 {
     if (!target_acquired()) {
         return false;
     }
 
-    ret = _target_pos;
+    Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
+
+    ret.x = _ekf_x.getPos()*100.0f + _inav.get_position().x + land_ofs_ned_cm.x;
+    ret.y = _ekf_y.getPos()*100.0f + _inav.get_position().y + land_ofs_ned_cm.y;
     return true;
 }
 
-bool AC_PrecLand::get_target_position_relative(Vector3f& ret)
+bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret) const
 {
     if (!target_acquired()) {
         return false;
     }
 
-    ret = _target_pos_rel;
-    return true;
-}
+    Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
 
-bool AC_PrecLand::get_target_velocity_relative(Vector3f& ret)
-{
-    return false;
+    ret.x = _ekf_x.getPos()*100.0f + land_ofs_ned_cm.x;
+    ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y;
+    return true;
 }
 
-// converts sensor's body-frame angles to earth-frame angles and position estimate
-//  raw sensor angles stored in _angle_to_target (might be in earth frame, or maybe body frame)
-//  earth-frame angles stored in _ef_angle_to_target
-//  position estimate is stored in _target_pos
-void AC_PrecLand::calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm)
+bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
 {
-    // rotate into NED frame
-    Vector3f target_vec_unit_ned = _ahrs.get_rotation_body_to_ned()*target_vec_unit_body;
-
-    // extract the angles to target (logging only)
-    _angle_to_target.x = atan2f(-target_vec_unit_body.y, target_vec_unit_body.z);
-    _angle_to_target.y = atan2f( target_vec_unit_body.x, target_vec_unit_body.z);
-    _ef_angle_to_target.x = atan2f(-target_vec_unit_ned.y, target_vec_unit_ned.z);
-    _ef_angle_to_target.y = atan2f( target_vec_unit_ned.x, target_vec_unit_ned.z);
-
-    if (target_vec_unit_ned.z > 0.0f) {
-        // get current altitude (constrained to be positive)
-        float alt = MAX(alt_above_terrain_cm, 0.0f);
-        float dist = alt/target_vec_unit_ned.z;
-        _target_pos_rel.x = target_vec_unit_ned.x*dist;
-        _target_pos_rel.y = target_vec_unit_ned.y*dist;
-        _target_pos_rel.z = alt;  // not used
-        _target_pos = _inav.get_position()+_target_pos_rel;
-
-        _last_update_ms = AP_HAL::millis();
+    if (!target_acquired()) {
+        return false;
     }
+    ret.x = _ekf_x.getVel()*100.0f;
+    ret.y = _ekf_y.getVel()*100.0f;
+    return true;
 }
 
 // handle_msg - Process a LANDING_TARGET mavlink message

libraries/AC_PrecLand/AC_PrecLand.h

@@ -6,6 +6,8 @@
 #include <AP_InertialNav/AP_InertialNav.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <stdint.h>
+#include "PosVelEKF.h"
+#include <AP_Buffer/AP_Buffer.h>
 
 // declare backend classes
 class AC_PrecLand_Backend;
@@ -42,49 +44,36 @@ public:
     void init();
 
     // returns true if precision landing is healthy
-    bool healthy() { return _backend_state.healthy; }
+    bool healthy() const { return _backend_state.healthy; }
 
     // returns time of last update
-    uint32_t last_update_ms() { return _last_update_ms; }
+    uint32_t last_update_ms() const { return _last_update_ms; }
 
     // give chance to driver to get updates from sensor
-    void update(float alt_above_terrain_cm);
-
-    // returns 3D vector of earth-frame position adjustments to target
-    Vector3f get_target_shift(const Vector3f& orig_target);
+    void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);
 
     // returns target position relative to origin
-    bool get_target_position(Vector3f& ret);
+    bool get_target_position_cm(Vector2f& ret) const;
 
     // returns target position relative to vehicle
-    bool get_target_position_relative(Vector3f& ret);
+    bool get_target_position_relative_cm(Vector2f& ret) const;
 
     // returns target velocity relative to vehicle
-    bool get_target_velocity_relative(Vector3f& ret);
+    bool get_target_velocity_relative_cms(Vector2f& ret) const;
 
     // returns true when the landing target has been detected
-    bool target_acquired();
+    bool target_acquired() const;
 
     // process a LANDING_TARGET mavlink message
     void handle_msg(mavlink_message_t* msg);
 
     // accessors for logging
     bool enabled() const { return _enabled; }
-    const Vector2f& last_bf_angle_to_target() const { return _angle_to_target; }
-    const Vector2f& last_ef_angle_to_target() const { return _ef_angle_to_target; }
-    const Vector3f& last_target_pos_offset() const { return _target_pos_rel; }
 
     // parameter var table
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
-
-    // converts sensor's body-frame angles to earth-frame angles and position estimate
-    //  angles stored in _angle_to_target
-    //  earth-frame angles stored in _ef_angle_to_target
-    //  position estimate is stored in _target_pos
-    void calc_angles_and_pos(const Vector3f& target_vec_unit_body, float alt_above_terrain_cm);
-
     // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
 
@@ -95,17 +84,17 @@ private:
     // parameters
     AP_Int8                     _enabled;           // enabled/disabled and behaviour
     AP_Int8                     _type;              // precision landing controller type
+    AP_Float                    _yaw_align;         // Yaw angle from body x-axis to sensor x-axis.
+    AP_Float                    _land_ofs_cm_x;     // Desired landing position of the camera forward of the target in vehicle body frame
+    AP_Float                    _land_ofs_cm_y;     // Desired landing position of the camera right of the target in vehicle body frame
 
     uint32_t                    _last_update_ms;      // epoch time in millisecond when update is called
     uint32_t                    _last_backend_los_meas_ms;
 
-    // output from sensor (stored for logging)
-    Vector2f                    _angle_to_target;   // last raw sensor angle to target
-    Vector2f                    _ef_angle_to_target;// last earth-frame angle to target
+    PosVelEKF                   _ekf_x, _ekf_y;
+    uint32_t                    _outlier_reject_count;
     
-    // estimator output
-    Vector3f                    _target_pos_rel;    // estimate target position relative to vehicle in NEU cm
-    Vector3f                    _target_pos;        // estimate target position in NEU cm
+    AP_Buffer<Matrix3f,8>       _attitude_history;
 
     // backend state
     struct precland_state {

libraries/AC_PrecLand/PosVelEKF.cpp

@@ -0,0 +1,93 @@
+#include "PosVelEKF.h"
+#include <math.h>
+#include <string.h>
+
+#define POSVELEKF_POS_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \
+__RET_NIS = ((-__X[0] + __Z)*(-__X[0] + __Z))/(__P[0] + __R);
+
+#define POSVELEKF_POS_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \
+__RET_STATE[0] = __P[0]*(-__X[0] + __Z)/(__P[0] + __R) + __X[0]; __RET_STATE[1] = __P[1]*(-__X[0] + \
+__Z)/(__P[0] + __R) + __X[1];
+
+#define POSVELEKF_POS_CALC_COV(__P, __R, __X, __Z, __RET_COV) \
+__RET_COV[0] = ((__P[0])*(__P[0]))*__R/((__P[0] + __R)*(__P[0] + __R)) + __P[0]*((-__P[0]/(__P[0] + \
+__R) + 1)*(-__P[0]/(__P[0] + __R) + 1)); __RET_COV[1] = __P[0]*__P[1]*__R/((__P[0] + __R)*(__P[0] + \
+__R)) - __P[0]*__P[1]*(-__P[0]/(__P[0] + __R) + 1)/(__P[0] + __R) + __P[1]*(-__P[0]/(__P[0] + __R) + \
+1); __RET_COV[2] = ((__P[1])*(__P[1]))*__R/((__P[0] + __R)*(__P[0] + __R)) - \
+((__P[1])*(__P[1]))/(__P[0] + __R) - __P[1]*(-__P[0]*__P[1]/(__P[0] + __R) + __P[1])/(__P[0] + __R) + \
+__P[2];
+
+#define POSVELEKF_PREDICTION_CALC_STATE(__P, __DT, __DV, __DV_NOISE, __X, __RET_STATE) \
+__RET_STATE[0] = __DT*__X[1] + __X[0]; __RET_STATE[1] = __DV + __X[1];
+
+#define POSVELEKF_PREDICTION_CALC_COV(__P, __DT, __DV, __DV_NOISE, __X, __RET_COV) \
+__RET_COV[0] = __DT*__P[1] + __DT*(__DT*__P[2] + __P[1]) + __P[0]; __RET_COV[1] = __DT*__P[2] + \
+__P[1]; __RET_COV[2] = ((__DV_NOISE)*(__DV_NOISE)) + __P[2];
+
+#define POSVELEKF_VEL_CALC_NIS(__P, __R, __X, __Z, __RET_NIS) \
+__RET_NIS = ((-__X[1] + __Z)*(-__X[1] + __Z))/(__P[2] + __R);
+
+#define POSVELEKF_VEL_CALC_STATE(__P, __R, __X, __Z, __RET_STATE) \
+__RET_STATE[0] = __P[1]*(-__X[1] + __Z)/(__P[2] + __R) + __X[0]; __RET_STATE[1] = __P[2]*(-__X[1] + \
+__Z)/(__P[2] + __R) + __X[1];
+
+#define POSVELEKF_VEL_CALC_COV(__P, __R, __X, __Z, __RET_COV) \
+__RET_COV[0] = __P[0] + ((__P[1])*(__P[1]))*__R/((__P[2] + __R)*(__P[2] + __R)) - \
+((__P[1])*(__P[1]))/(__P[2] + __R) - __P[1]*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1])/(__P[2] + __R); \
+__RET_COV[1] = __P[1]*__P[2]*__R/((__P[2] + __R)*(__P[2] + __R)) + (-__P[2]/(__P[2] + __R) + \
+1)*(-__P[1]*__P[2]/(__P[2] + __R) + __P[1]); __RET_COV[2] = ((__P[2])*(__P[2]))*__R/((__P[2] + \
+__R)*(__P[2] + __R)) + __P[2]*((-__P[2]/(__P[2] + __R) + 1)*(-__P[2]/(__P[2] + __R) + 1));
+
+void PosVelEKF::init(float pos, float posVar, float vel, float velVar)
+{
+    _state[0] = pos;
+    _state[1] = vel;
+    _cov[0] = posVar;
+    _cov[1] = 0.0f;
+    _cov[2] = velVar;
+}
+
+void PosVelEKF::predict(float dt, float dVel, float dVelNoise)
+{
+    float newState[2];
+    float newCov[3];
+
+    POSVELEKF_PREDICTION_CALC_STATE(_cov, dt, dVel, dVelNoise, _state, newState)
+    POSVELEKF_PREDICTION_CALC_COV(_cov, dt, dVel, dVelNoise, _state, newCov)
+
+    memcpy(_state,newState,sizeof(_state));
+    memcpy(_cov,newCov,sizeof(_cov));
+}
+
+void PosVelEKF::fusePos(float pos, float posVar)
+{
+    float newState[2];
+    float newCov[3];
+
+    POSVELEKF_POS_CALC_STATE(_cov, posVar, _state, pos, newState)
+    POSVELEKF_POS_CALC_COV(_cov, posVar, _state, pos, newCov)
+
+    memcpy(_state,newState,sizeof(_state));
+    memcpy(_cov,newCov,sizeof(_cov));
+}
+
+void PosVelEKF::fuseVel(float vel, float velVar)
+{
+    float newState[2];
+    float newCov[3];
+
+    POSVELEKF_VEL_CALC_STATE(_cov, velVar, _state, vel, newState)
+    POSVELEKF_VEL_CALC_COV(_cov, velVar, _state, vel, newCov)
+
+    memcpy(_state,newState,sizeof(_state));
+    memcpy(_cov,newCov,sizeof(_cov));
+}
+
+float PosVelEKF::getPosNIS(float pos, float posVar)
+{
+    float ret;
+
+    POSVELEKF_POS_CALC_NIS(_cov, posVar, _state, pos, ret)
+
+    return ret;
+}

libraries/AC_PrecLand/PosVelEKF.h

@@ -0,0 +1,19 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+#pragma once
+
+class PosVelEKF {
+public:
+    void init(float pos, float posVar, float vel, float velVar);
+    void predict(float dt, float dVel, float dVelNoise);
+    void fusePos(float pos, float posVar);
+    void fuseVel(float vel, float velVar);
+
+    float getPos() const { return _state[0]; }
+    float getVel() const { return _state[1]; }
+
+    float getPosNIS(float pos, float posVar);
+
+private:
+    float _state[2];
+    float _cov[3];
+};