AC_PrecLand: rework estimation, add EST_TYPE param

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -50,7 +50,13 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = {
     // @Units: Centimeters
     AP_GROUPINFO("LAND_OFS_Y",    4, AC_PrecLand, _land_ofs_cm_y, 0),
 
-    // 5 RESERVED for EKF_TYPE
+    // @Param: EST_TYPE
+    // @DisplayName: Precision Land Estimator Type
+    // @Description: Specifies the estimation method used.
+    // @Values: 0:RAW_MEASUREMENTS, 1:TWO_STATE_KF_PER_AXIS
+    // @User: Advanced
+    AP_GROUPINFO("EST_TYPE",    5, AC_PrecLand, _estimator_type, 1),
+
     // 6 RESERVED for ACC_NSE
 
     AP_GROUPEND
@@ -64,6 +70,7 @@ AC_PrecLand::AC_PrecLand(const AP_AHRS& ahrs, const AP_InertialNav& inav) :
     _ahrs(ahrs),
     _inav(inav),
     _last_update_ms(0),
+    _target_acquired(false),
     _last_backend_los_meas_ms(0),
     _backend(nullptr)
 {
@@ -122,124 +129,51 @@ void AC_PrecLand::init()
 // update - give chance to driver to get updates from sensor
 void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid)
 {
-    _attitude_history.push_back(_ahrs.get_rotation_body_to_ned());
+    // Collect inertial data and append it to the buffer
+    struct inertial_data_frame_s inertial_data_newest;
+    _ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt);
+    inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned();
+    inertial_data_newest.inertialNavVelocity = _inav.get_velocity()*0.01f;
+    inertial_data_newest.inertialNavVelocityValid = _inav.get_filter_status().flags.horiz_vel;
+    _inertial_history.push_back(inertial_data_newest);
     
-    // run backend update
     if (_backend != nullptr && _enabled) {
-        // read from sensor
         _backend->update();
-
+        run_estimator(rangefinder_alt_cm*0.01f, rangefinder_alt_valid);
-        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 (_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();
-
-            Vector3f target_vec_unit_body;
-            _backend->get_los_body(target_vec_unit_body);
-
-            // 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;
-            bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_cm > 0.0f) || (_backend->distance_to_target() > 0.0f);
-
-            if (target_vec_valid && alt_valid) {
-                float alt;
-                if (_backend->distance_to_target() > 0.0f) {
-                    alt = _backend->distance_to_target();
-                } else {
-                    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(2.0f));
-                        _ekf_y.init(targetPosRelMeasNED.y, xy_pos_var, -vehicleVelocityNED.y, sq(2.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() const
+bool AC_PrecLand::target_acquired()
 {
-    return (AP_HAL::millis()-_last_update_ms) < 2000;
+    _target_acquired = _target_acquired && (AP_HAL::millis()-_last_update_ms) < 2000;
+    return _target_acquired;
 }
 
-bool AC_PrecLand::get_target_position_cm(Vector2f& ret) const
+bool AC_PrecLand::get_target_position_cm(Vector2f& ret)
 {
     if (!target_acquired()) {
         return false;
     }
-
+    ret.x = _target_pos_rel_out_NE.x*100.0f + _inav.get_position().x;
-    Vector3f land_ofs_ned_cm = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0);
+    ret.y = _target_pos_rel_out_NE.y*100.0f + _inav.get_position().y;
-
-    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_cm(Vector2f& ret) const
+bool AC_PrecLand::get_target_position_relative_cm(Vector2f& ret)
 {
     if (!target_acquired()) {
         return false;
     }
-
+    ret = _target_pos_rel_out_NE*100.0f;
-    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 + land_ofs_ned_cm.x;
-    ret.y = _ekf_y.getPos()*100.0f + land_ofs_ned_cm.y;
     return true;
 }
 
-bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret) const
+bool AC_PrecLand::get_target_velocity_relative_cms(Vector2f& ret)
 {
     if (!target_acquired()) {
         return false;
     }
-    ret.x = _ekf_x.getVel()*100.0f;
+    ret = _target_vel_rel_out_NE*100.0f;
-    ret.y = _ekf_y.getVel()*100.0f;
     return true;
 }
 
@@ -251,3 +185,172 @@ void AC_PrecLand::handle_msg(mavlink_message_t* msg)
         _backend->handle_msg(msg);
     }
 }
+
+//
+// Private methods
+//
+
+void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid)
+{
+    const struct inertial_data_frame_s& inertial_data_delayed = _inertial_history.front();
+
+    switch (_estimator_type) {
+        case ESTIMATOR_TYPE_RAW_MEASUREMENTS: {
+            // Return if there's any invalid velocity data
+            for (uint8_t i=0; i<_inertial_history.size(); i++) {
+                const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
+                if (!inertial_data.inertialNavVelocityValid) {
+                    _target_acquired = false;
+                    return;
+                }
+            }
+
+            // Predict
+            if (target_acquired()) {
+                _target_pos_rel_est_NE.x -= inertial_data_delayed.inertialNavVelocity.x * inertial_data_delayed.dt;
+                _target_pos_rel_est_NE.y -= inertial_data_delayed.inertialNavVelocity.y * inertial_data_delayed.dt;
+                _target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
+                _target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
+            }
+
+            // Update if a new LOS measurement is available
+            Vector3f target_vec_unit_body;
+            if (retrieve_los_meas(target_vec_unit_body)) {
+                Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body;
+                bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
+                bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
+                if (target_vec_valid && alt_valid) {
+                    float alt;
+                    if (_backend->distance_to_target() > 0.0f) {
+                        alt = _backend->distance_to_target();
+                    } else {
+                        alt = MAX(rangefinder_alt_m, 0.0f);
+                    }
+                    float dist = alt/target_vec_unit_ned.z;
+                    _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
+
+                    _target_pos_rel_est_NE.x = _target_pos_rel_meas_NED.x;
+                    _target_pos_rel_est_NE.y = _target_pos_rel_meas_NED.y;
+                    _target_vel_rel_est_NE.x = -inertial_data_delayed.inertialNavVelocity.x;
+                    _target_vel_rel_est_NE.y = -inertial_data_delayed.inertialNavVelocity.y;
+
+                    _last_update_ms = AP_HAL::millis();
+                    _target_acquired = true;
+                }
+            }
+
+            // Output prediction
+            if (target_acquired()) {
+                run_output_prediction();
+            }
+            break;
+        }
+        case ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS: {
+            // Predict
+            if (target_acquired()) {
+                const float& dt = inertial_data_delayed.dt;
+                const Vector3f& vehicleDelVel = inertial_data_delayed.correctedVehicleDeltaVelocityNED;
+
+                _ekf_x.predict(dt, -vehicleDelVel.x, 0.5f*dt);
+                _ekf_y.predict(dt, -vehicleDelVel.y, 0.5f*dt);
+            }
+
+            // Update if a new LOS measurement is available
+            Vector3f target_vec_unit_body;
+            if (retrieve_los_meas(target_vec_unit_body)) {
+                Vector3f target_vec_unit_ned = inertial_data_delayed.Tbn * target_vec_unit_body;
+                bool target_vec_valid = target_vec_unit_ned.z > 0.0f;
+                bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
+                if (target_vec_valid && alt_valid) {
+                    float alt;
+                    if (_backend->distance_to_target() > 0.0f) {
+                        alt = _backend->distance_to_target();
+                    } else {
+                        alt = MAX(rangefinder_alt_m, 0.0f);
+                    }
+                    float dist = alt/target_vec_unit_ned.z;
+                    _target_pos_rel_meas_NED = Vector3f(target_vec_unit_ned.x*dist, target_vec_unit_ned.y*dist, alt);
+
+                    float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f);
+                    if (!target_acquired()) {
+                        // reset filter state
+                        if (inertial_data_delayed.inertialNavVelocityValid) {
+                            _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.x, sq(2.0f));
+                            _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, -inertial_data_delayed.inertialNavVelocity.y, sq(2.0f));
+                        } else {
+                            _ekf_x.init(_target_pos_rel_meas_NED.x, xy_pos_var, 0.0f, sq(10.0f));
+                            _ekf_y.init(_target_pos_rel_meas_NED.y, xy_pos_var, 0.0f, sq(10.0f));
+                        }
+                        _last_update_ms = AP_HAL::millis();
+                        _target_acquired = true;
+                    } else {
+                        float NIS_x = _ekf_x.getPosNIS(_target_pos_rel_meas_NED.x, xy_pos_var);
+                        float NIS_y = _ekf_y.getPosNIS(_target_pos_rel_meas_NED.y, xy_pos_var);
+                        if (MAX(NIS_x, NIS_y) < 3.0f || _outlier_reject_count >= 3) {
+                            _outlier_reject_count = 0;
+                            _ekf_x.fusePos(_target_pos_rel_meas_NED.x, xy_pos_var);
+                            _ekf_y.fusePos(_target_pos_rel_meas_NED.y, xy_pos_var);
+                            _last_update_ms = AP_HAL::millis();
+                            _target_acquired = true;
+                        } else {
+                            _outlier_reject_count++;
+                        }
+                    }
+                }
+            }
+
+            // Output prediction
+            if (target_acquired()) {
+                _target_pos_rel_est_NE.x = _ekf_x.getPos();
+                _target_pos_rel_est_NE.y = _ekf_y.getPos();
+                _target_vel_rel_est_NE.x = _ekf_x.getVel();
+                _target_vel_rel_est_NE.y = _ekf_y.getVel();
+
+                run_output_prediction();
+            }
+            break;
+        }
+    }
+}
+
+bool AC_PrecLand::retrieve_los_meas(Vector3f& target_vec_unit_body)
+{
+    if (_backend->have_los_meas() && _backend->los_meas_time_ms() != _last_backend_los_meas_ms) {
+        _last_backend_los_meas_ms = _backend->los_meas_time_ms();
+        _backend->get_los_body(target_vec_unit_body);
+
+        // 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
+        );
+
+        target_vec_unit_body = Rz*target_vec_unit_body;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+void AC_PrecLand::run_output_prediction()
+{
+    _target_pos_rel_out_NE = _target_pos_rel_est_NE;
+    _target_vel_rel_out_NE = _target_vel_rel_est_NE;
+
+    // Predict forward from delayed time horizon
+    for (uint8_t i=1; i<_inertial_history.size(); i++) {
+        const struct inertial_data_frame_s& inertial_data = _inertial_history.peek(i);
+        _target_vel_rel_out_NE.x -= inertial_data.correctedVehicleDeltaVelocityNED.x;
+        _target_vel_rel_out_NE.y -= inertial_data.correctedVehicleDeltaVelocityNED.y;
+        _target_pos_rel_out_NE.x += _target_vel_rel_out_NE.x * inertial_data.dt;
+        _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data.dt;
+    }
+
+    // Apply offset
+    Vector3f land_ofs_ned_m = _ahrs.get_rotation_body_to_ned() * Vector3f(_land_ofs_cm_x,_land_ofs_cm_y,0) * 0.01f;
+    _target_pos_rel_out_NE.x += land_ofs_ned_m.x;
+    _target_pos_rel_out_NE.y += land_ofs_ned_m.y;
+}

libraries/AC_PrecLand/AC_PrecLand.h

@@ -58,16 +58,16 @@ public:
     void update(float rangefinder_alt_cm, bool rangefinder_alt_valid);
 
     // returns target position relative to origin
-    bool get_target_position_cm(Vector2f& ret) const;
+    bool get_target_position_cm(Vector2f& ret);
 
     // returns target position relative to vehicle
-    bool get_target_position_relative_cm(Vector2f& ret) const;
+    bool get_target_position_relative_cm(Vector2f& ret);
 
     // returns target velocity relative to vehicle
-    bool get_target_velocity_relative_cms(Vector2f& ret) const;
+    bool get_target_velocity_relative_cms(Vector2f& ret);
 
     // returns true when the landing target has been detected
-    bool target_acquired() const;
+    bool target_acquired();
 
     // process a LANDING_TARGET mavlink message
     void handle_msg(mavlink_message_t* msg);
@@ -79,27 +79,54 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
 private:
+    enum estimator_type_t {
+        ESTIMATOR_TYPE_RAW_MEASUREMENTS=0,
+        ESTIMATOR_TYPE_TWO_STATE_KF_PER_AXIS=1
+    };
+
     // returns enabled parameter as an behaviour
     enum PrecLandBehaviour get_behaviour() const { return (enum PrecLandBehaviour)(_enabled.get()); }
 
+    void run_estimator(float rangefinder_alt_m, bool rangefinder_alt_valid);
+    bool retrieve_los_meas(Vector3f& target_vec_unit_body);
+    void run_output_prediction();
+
     // references to inertial nav and ahrs libraries
     const AP_AHRS&              _ahrs;
     const AP_InertialNav&       _inav;
 
     // parameters
     AP_Int8                     _enabled;           // enabled/disabled and behaviour
-    AP_Int8                     _type;              // precision landing controller type
+    AP_Int8                     _type;              // precision landing sensor type
+    AP_Int8                     _estimator_type;    // precision landing estimator 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
+    bool                        _target_acquired;
     uint32_t                    _last_backend_los_meas_ms;
 
     PosVelEKF                   _ekf_x, _ekf_y;
     uint32_t                    _outlier_reject_count;
     
-    AP_Buffer<Matrix3f,8>       _attitude_history;
+    Vector3f                    _target_pos_rel_meas_NED;
+
+    Vector2f                    _target_pos_rel_est_NE;
+    Vector2f                    _target_vel_rel_est_NE;
+
+    Vector2f                    _target_pos_rel_out_NE;
+    Vector2f                    _target_vel_rel_out_NE;
+
+    struct inertial_data_frame_s {
+        Matrix3f Tbn;
+        Vector3f correctedVehicleDeltaVelocityNED;
+        Vector3f inertialNavVelocity;
+        bool inertialNavVelocityValid;
+        float dt;
+    };
+
+    AP_Buffer<inertial_data_frame_s,8>       _inertial_history;
 
     // backend state
     struct precland_state {