FAST LIVO2 EKF2 fusion lost after 1 minute

[ddinocoe]

Hi all,

I’m trying to achieve GNSS-denied flight using external vision with the following setup:

• External vision / odometry: FAST-LIVO2

• LiDAR: Livox Mid-360

• Camera: OAK-D W

• FCU: MicoAir743 V2

• PX4 firmware: v1.16

The visual odometry output itself appears to be stable and accurate. However, after about 1 minute of operation, EKF2 stops fusing the external vision data, despite the continuous publishing of accurate odometry from FAST-LIVO2

The interesting part is that this happens even when the drone is completely stationary and not armed. If I move the drone around after fusion has stopped, EKF2 can sometimes start fusing the external vision again for another period of time, so I’m curious as to what makes it lose it in the first place or regains after.

Since the dropout happens at roughly the same time regardless of whether the vehicle is moving, I’m wondering whether there is some EKF2 timeout, reset condition, timestamp issue, or some condition in the EV fusion control logic that I might be missing.

Here is an example flight log https://review.px4.io/plot_app?log=701e1aaa-75c9-4a99-a44f-690bbb401b61 and its estimator status flags and estimator vs visual odometry

I can provide more flight logs or the full parameter list if required.

Thanks!

[yigitcan-ozturk]

Interesting case, especially because the fusion loss also occurs while the vehicle is stationary and disarmed.

I would avoid assuming that the ~1 minute interval itself represents a fixed EKF2 timeout until the first actual fusion-control transition is identified in the log.

One useful way to narrow this down would be to reconstruct the sequence around the first EV fusion loss, rather than starting from the later estimator behaviour.

I would check:

• whether vehicle_visual_odometry timestamps continue advancing normally,
• sample arrival intervals and any gap immediately before fusion stops,
• EV position/velocity/yaw innovation and test-ratio behaviour,
• estimator_status_flags transitions related to EV fusion,
• whether the EV reset counters or reported variances change,
• and the exact timestamp at which EKF2 stops accepting/fusing the measurements.

The fact that moving the vehicle can sometimes make fusion resume is particularly interesting. It may help distinguish “EV data stopped arriving” from “data continued arriving but EKF2 stopped accepting it”.

If you can provide the parameter file as well, that would help correlate the fusion configuration with the ULog.

I’d be interested in looking at the log around the first transition and identifying which observable condition changes first.

[ddinocoe]

Thank you for the reply, I have logged down a stationary scenario from boot till shutdown to better look at the problem.
Here is the log file https://review.px4.io/plot_app?log=27d93abe-eec9-43d6-ad78-cbbd4ddeea2f and parameter list (had to make it into a pdf).

Full list parameters.pdf (209.5 KB)

To answer your checklist,

• Yes the vehicle_visual_odometry timestamps continue to advance normally

• Both the timestamp and timestamp_sample don’t show any obvious gaps.

• I don’t fuse the velocity only height, position and yaw, and the innovation and test ratio behaviors don’t seem to have anything out of the ordinary

  

• The estimator_status_flags can be seen in the image above as well. The interesting thing is that it regained fusion before I shut it down.

• The EV variances are hardcoded in my bridge and remain constant at 0.01, so there is no variance change associated with the fusion loss. The reset counter from the EV also remains at 0. However, I have recorded the estimator_status reset counts.

  

• I would say the exact timestamp where all three flags go down is at around 134.87 s

The problem could also be from my bridge code, which if required, I can also paste it here.

[yigitcan-ozturk]

Thanks, this log narrows it down quite a bit.

The fact that timestamp and timestamp_sample continue normally, the EV variances remain constant, and the EV reset_counter stays at 0 makes a simple EV stream interruption or an explicit estimator reset less likely.

The most interesting part is that position, height and yaw fusion appear to drop together at approximately 134.87 s and later recover. That suggests we should look for a common condition affecting EV fusion rather than treating the three fusion paths independently.

Could you paste the bridge code that publishes vehicle_visual_odometry?

In particular, I’d like to check how these fields are populated:

• timestamp / timestamp_sample

• pose_frame and velocity_frame

• position and quaternion

• position/orientation variance

• quality

• reset_counter

Also, around 134.87 s, could you check whether vehicle_visual_odometry.quality, the pose frame, or any validity-related field changes even for a single sample?

If you can paste the publisher/bridge section, I can trace it against the EKF2 EV fusion conditions. The simultaneous loss and later recovery should give us a fairly small search space.

[ddinocoe]

Here is the bridge code, the quaternions are manually converted due to the lidar having a 90° transform with respect to the FCU

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <px4_msgs/msg/vehicle_odometry.hpp>
#include <px4_ros_com/frame_transforms.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
#include <limits>

class OdomBridge : public rclcpp::Node
{
public:
    OdomBridge() : Node("odom_pub")
    {
        rclcpp::QoS qos_sub(10);
        qos_sub.best_effort();

        sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
            "/mavros/vision_pose/pose",
            qos_sub,
            std::bind(&OdomBridge::pose_callback, this, std::placeholders::_1));

        pub_px4_ = this->create_publisher<px4_msgs::msg::VehicleOdometry>(
            "/fmu/in/vehicle_visual_odometry",
            10);

        //pub_debug_ = this->create_publisher<nav_msgs::msg::Odometry>(
        //    "/debug/vehicle_visual_odometry",
        //    10);
    }

private:
    void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
        px4_msgs::msg::VehicleOdometry px4_msg{};

        uint64_t now_us = this->get_clock()->now().nanoseconds() / 1000;
        
        uint64_t sample_us =
            static_cast<uint64_t>(msg->header.stamp.sec) * 1000000ULL +
            static_cast<uint64_t>(msg->header.stamp.nanosec) / 1000ULL;

        px4_msg.timestamp = now_us;
        // px4_msg.timestamp_sample = now_us;
        px4_msg.timestamp_sample = sample_us;

        px4_msg.pose_frame = px4_msgs::msg::VehicleOdometry::POSE_FRAME_FRD;
        px4_msg.velocity_frame = px4_msgs::msg::VehicleOdometry::VELOCITY_FRAME_UNKNOWN;

        const double yaw_deg = 0.0;
        const double yaw_rad = yaw_deg * M_PI / 180.0;

        Eigen::AngleAxisd yaw_correction_axis(yaw_rad, Eigen::Vector3d::UnitZ());
        Eigen::Quaterniond q_mount_correction_yaw(yaw_correction_axis);

        // Position from PoseStamped, assumed ROS ENU
        Eigen::Vector3d p_enu(
            msg->pose.position.x,
            msg->pose.position.y,
            msg->pose.position.z
        );

        // No manual mount correction for position for now
        Eigen::Vector3d p_ned =
            px4_ros_com::frame_transforms::enu_to_ned_local_frame(q_mount_correction_yaw * p_enu);

        // Orientation from PoseStamped
        Eigen::Quaterniond q_enu(
            msg->pose.orientation.w,
            msg->pose.orientation.x,
            msg->pose.orientation.y,
            msg->pose.orientation.z
        );

        q_enu.normalize();

        // No manual pitch/yaw/roll mount correction for orientation.
        // // This only converts ROS orientation convention to PX4 convention.
        // Eigen::Quaterniond q_px4 =
        //     px4_ros_com::frame_transforms::ros_to_px4_orientation(q_mount_correction_yaw * q_enu);

        px4_msg.position[0] = static_cast<float>(p_ned.x());
        px4_msg.position[1] = static_cast<float>(p_ned.y());
        px4_msg.position[2] = static_cast<float>(p_ned.z());

        px4_msg.q[0] = static_cast<float>(q_enu.w());
        px4_msg.q[1] = static_cast<float>(q_enu.y());
        px4_msg.q[2] = static_cast<float>(q_enu.x());
        px4_msg.q[3] = static_cast<float>(-q_enu.z());

        px4_msg.position_variance[0] = 0.01f;
        px4_msg.position_variance[1] = 0.01f;
        px4_msg.position_variance[2] = 0.01f;

        px4_msg.orientation_variance[0] = 0.01f;
        px4_msg.orientation_variance[1] = 0.01f;
        px4_msg.orientation_variance[2] = 0.01f;

        const float NaN = std::numeric_limits<float>::quiet_NaN();

        px4_msg.velocity[0] = NaN;
        px4_msg.velocity[1] = NaN;
        px4_msg.velocity[2] = NaN;

        px4_msg.angular_velocity[0] = NaN;
        px4_msg.angular_velocity[1] = NaN;
        px4_msg.angular_velocity[2] = NaN;

        px4_msg.velocity_variance[0] = NaN;
        px4_msg.velocity_variance[1] = NaN;
        px4_msg.velocity_variance[2] = NaN;

        px4_msg.reset_counter = 0;
        px4_msg.quality = 100;

        pub_px4_->publish(px4_msg);

        // Publish debug odometry for RViz2
        // nav_msgs::msg::Odometry dbg_msg;
        // dbg_msg.header.stamp = this->get_clock()->now();
        // dbg_msg.header.frame_id = "global";
        // dbg_msg.child_frame_id = "imu";

        // dbg_msg.pose.pose.position.x = px4_msg.position[0];
        // dbg_msg.pose.pose.position.y = px4_msg.position[1];
        // dbg_msg.pose.pose.position.z = px4_msg.position[2];

        // dbg_msg.pose.pose.orientation.w = px4_msg.q[0];
        // dbg_msg.pose.pose.orientation.x = px4_msg.q[1];
        // dbg_msg.pose.pose.orientation.y = px4_msg.q[2];
        // dbg_msg.pose.pose.orientation.z = px4_msg.q[3];

        // pub_debug_->publish(dbg_msg);
    }

    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_;
    rclcpp::Publisher<px4_msgs::msg::VehicleOdometry>::SharedPtr pub_px4_;
    // rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_debug_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<OdomBridge>());
    rclcpp::shutdown();
    return 0;
}

I’ve changed the pose frame between NED and FRD and both have the same problem still.
As for the quality and pose_frame around 134.87 s are at constant 100 and 2 respectively. Some other validity field i can think of are some flags from estimator_status_flags such as the reject flags which are 0, cs_tilt_align is 1, and cs_yaw_align is also 1 for NED but 0 for FRD (but that is not the problem as the same fusion dropout occurs with pose frame=NED).

[yigitcan-ozturk]

Thanks the bridge code reveals a couple of things that I would isolate before looking for an EKF2-side bug.

One thing stands out immediately: the position is explicitly converted from ENU to NED:

Eigen::Vector3d p_ned =
    px4_ros_com::frame_transforms::enu_to_ned_local_frame(...);

but the message is then labelled:

px4_msg.pose_frame =
    px4_msgs::msg::VehicleOdometry::POSE_FRAME_FRD;

Those aren’t equivalent frames from EKF2’s perspective. POSE_FRAME_NED is North-aligned, while POSE_FRAME_FRD represents a local FRD frame with an arbitrary fixed heading offset. EKF2 also handles those two cases differently, including interaction with EV yaw alignment.

So for the first controlled test I would make the whole message consistently NED:

px4_msg.pose_frame =
    px4_msgs::msg::VehicleOdometry::POSE_FRAME_NED;

Eigen::Vector3d p_ned =
    px4_ros_com::frame_transforms::enu_to_ned_local_frame(p_enu);

Eigen::Quaterniond q_px4 =
    px4_ros_com::frame_transforms::ros_to_px4_orientation(q_enu);

px4_msg.q[0] = q_px4.w();
px4_msg.q[1] = q_px4.x();
px4_msg.q[2] = q_px4.y();
px4_msg.q[3] = q_px4.z();

That also removes the manual quaternion component permutation as another variable.

The second thing I would instrument is the timestamp relationship. timestamp_sample is coming directly from the incoming PoseStamped, while timestamp comes from the ROS node clock. The fact that both advance continuously doesn’t necessarily prove that they’re in the same clock domain.

Could you log around the failure:

timestamp
timestamp_sample
timestamp - timestamp_sample

especially from ~125 s to ~145 s?

I would also try one diagnostic run with:

px4_msg.timestamp_sample = now_us;

not as the final latency-correct implementation, but simply as an A/B test. If the ~134.87 s fusion loss disappears, that would strongly point us toward timestamp/clock-domain handling.

The fact that quality remains 100, reset_counter remains 0, reject flags remain 0, and all three EV fusion flags disappear together and later recover makes this look more like a common EV eligibility/alignment/timing condition than three independent innovation failures.

Also, your observation that cs_yaw_align differs between the NED and FRD cases is useful. I would keep NED/FRD as separate experiments rather than changing only the enum while feeding otherwise NED-transformed data.

If you can run the consistent-NED + timestamp_sample = now_us diagnostic case and post whether the dropout still occurs, that should split the problem into two much smaller branches.