AP_VisualOdom: support covariance from msg and add pos, ang err parameters

libraries/AP_VisualOdom/AP_VisualOdom.cpp

@@ -90,6 +90,22 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_VEL_M_NSE", 5, AP_VisualOdom, _vel_noise, 0.1),
 
+    // @Param: _POS_M_NSE
+    // @DisplayName: Visual odometry position measurement noise
+    // @Description: Visual odometry position measurement noise in meters
+    // @Units: m
+    // @Range: 0.1 10.0
+    // @User: Advanced
+    AP_GROUPINFO("_POS_M_NSE", 6, AP_VisualOdom, _pos_noise, 0.2f),
+
+    // @Param: _YAW_M_NSE
+    // @DisplayName: Visual odometry yaw measurement noise
+    // @Description: Visual odometry yaw measurement noise in radians
+    // @Units: rad
+    // @Range: 0.05 1.0
+    // @User: Advanced
+    AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
+
     AP_GROUPEND
 };
 
@@ -156,7 +172,7 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
 
 // general purpose method to consume position estimate data and send to EKF
 // distances in meters, roll, pitch and yaw are in radians
-void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter)
+void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -168,7 +184,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
         // convert attitude to quaternion and call backend
         Quaternion attitude;
         attitude.from_euler(roll, pitch, yaw);
-        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
+        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
     }
 }
 
@@ -182,7 +198,7 @@ void AP_VisualOdom::handle_vision_position_estimate(uint64_t remote_time_us, uin
 
     // call backend
     if (_driver != nullptr) {
-        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, reset_counter);
+        _driver->handle_vision_position_estimate(remote_time_us, time_ms, x, y, z, attitude, 0, 0, reset_counter);
     }
 }
 

libraries/AP_VisualOdom/AP_VisualOdom.h

@@ -73,13 +73,19 @@ public:
 
     // return velocity measurement noise in m/s
     float get_vel_noise() const { return _vel_noise; }
+    
+    // return position measurement noise in m
+    float get_pos_noise() const { return _pos_noise; }
+
+    // return yaw measurement noise in rad
+    float get_yaw_noise() const { return _yaw_noise; }
 
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
 
     // general purpose methods to consume position estimate data and send to EKF
     // distances in meters, roll, pitch and yaw are in radians
-    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, uint8_t reset_counter);
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
     void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter);
     
     // general purpose methods to consume velocity estimate data and send to EKF
@@ -105,6 +111,8 @@ private:
     AP_Float _pos_scale;        // position scale factor applied to sensor values
     AP_Int16 _delay_ms;         // average delay relative to inertial measurements
     AP_Float _vel_noise;        // velocity measurement noise in m/s
+    AP_Float _pos_noise;        // position measurement noise in meters
+    AP_Float _yaw_noise;        // yaw measurement noise in radians
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;

libraries/AP_VisualOdom/AP_VisualOdom_Backend.h

@@ -31,7 +31,7 @@ public:
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
 
     // consume vision position estimate data and send to EKF. distances in meters
-    virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) = 0;
+    virtual void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
     virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

@@ -24,7 +24,7 @@
 extern const AP_HAL::HAL& hal;
 
 // consume vision position estimate data and send to EKF. distances in meters
-void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
+void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
 {
     const float scale_factor = _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@@ -41,9 +41,9 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     rotate_and_correct_position(pos);
     rotate_attitude(att);
 
+    posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
+    angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
     // send attitude and position to EKF
-    const float posErr = 0; // parameter required?
-    const float angErr = 0; // parameter required?
     AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
 
     // calculate euler orientation for logging
@@ -53,7 +53,7 @@ void AP_VisualOdom_IntelT265::handle_vision_position_estimate(uint64_t remote_ti
     att.to_euler(roll, pitch, yaw);
 
     // log sensor data
-    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), reset_counter);
+    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter);
 
     // store corrected attitude for use in pre-arm checks
     _attitude_last = att;

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h

@@ -12,7 +12,7 @@ public:
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
     // consume vision position estimate data and send to EKF. distances in meters
-    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
     void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;

libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp

@@ -30,14 +30,14 @@ AP_VisualOdom_MAV::AP_VisualOdom_MAV(AP_VisualOdom &frontend) :
 }
 
 // consume vision position estimate data and send to EKF. distances in meters
-void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter)
+void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
 {
     const float scale_factor =  _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
 
+    posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
+    angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
     // send attitude and position to EKF
-    const float posErr = 0; // parameter required?
-    const float angErr = 0; // parameter required?
     AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
 
     // calculate euler orientation for logging
@@ -47,7 +47,7 @@ void AP_VisualOdom_MAV::handle_vision_position_estimate(uint64_t remote_time_us,
     attitude.to_euler(roll, pitch, yaw);
 
     // log sensor data
-    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), reset_counter);
+    AP::logger().Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter);
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();

libraries/AP_VisualOdom/AP_VisualOdom_MAV.h

@@ -12,7 +12,7 @@ public:
     AP_VisualOdom_MAV(AP_VisualOdom &frontend);
 
     // consume vision position estimate data and send to EKF. distances in meters
-    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, uint8_t reset_counter) override;
+    void handle_vision_position_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
     void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;