扩展卡尔曼滤波学习（三）

Eigen库安装参考：VS2013配置Eigen+Opencv_eigen vs opencv-CSDN博客

扩展卡尔曼程序涉及参考：开发者说丨手把手教你写扩展卡尔曼滤波器-百度开发者中心 

这个程序错误较多，且和我想要的模型相差甚远，最后还是使用了文心一言生成模型，kimiai修改代码，才让模型达到了我想要的效果。

贴一下我的代码吧~~~

#pragma once
#include "Eigen/Dense"
#include <opencv2/video/tracking.hpp>// 包含卡尔曼滤波器  
#include <opencv2/highgui/highgui.hpp>
#include "math.h"
#include <iostream>
 

using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
class ExtendedKalmanFilter {
public:
    ExtendedKalmanFilter() {
        // 初始化状态向量和协方差矩阵  
        x = Eigen::VectorXd(4);
        x << 0, 0, 0, 0; // 初始位置(0,0)和速度(0,0)  
        P = Eigen::MatrixXd::Identity(4, 4) * 100.0; // 增大初始协方差以反映更大的不确定性

        // 初始化过程噪声和测量噪声矩阵  
        Q = Eigen::MatrixXd::Zero(4, 4);
        Q << 0.5, 0, 0.5, 0,   //x方向噪声
            0, 0.5, 0, 0.5,    //y方向噪声
            0.5, 0, 1, 0,      //vx方向噪声
            0, 0.5, 0, 1;      //vy方向噪声

        R = Eigen::MatrixXd::Identity(2, 2) * 0.01; // 测量噪声矩阵  

        // 初始化转移矩阵F和测量矩阵H  
        F = Eigen::MatrixXd::Identity(4, 4);
        F(0, 2) = F(1, 3) = 1.0; // 假设匀速运动  

        H = Eigen::MatrixXd::Zero(2, 4);
        H << 1, 0, 0, 0,
            0, 1, 0, 0; // 只测量x和y位置  

        is_initialized = true;
    }

    void Prediction(double delta_t) {
        // 预测状态  
        Eigen::MatrixXd F_t = F * delta_t; // 离散时间下的F  
        x = F_t * x;

        // 预测协方差  
        P = F_t * P * F_t.transpose() + Q;
    }

    Eigen::VectorXd MeasurementUpdate(const Eigen::VectorXd& z) {
        // 计算残差  
        Eigen::VectorXd y = z - H * x;

        // 计算卡尔曼增益  
        Eigen::MatrixXd S = H * P * H.transpose() + R;
        Eigen::MatrixXd K = P * H.transpose() * S.inverse();

        // 更新状态  
        x = x + K * y;

        // 更新协方差  
        P = (Eigen::MatrixXd::Identity(4, 4) - K * H) * P;

        // 返回更新后的x和y  
        return x.head(2);
    }

    Eigen::VectorXd GetX() {
        return x;
    }

    bool IsInitialized() {
        return is_initialized;
    }

private:
    bool is_initialized;
    Eigen::VectorXd x; // 状态向量 [x, y, vx, vy]  
    Eigen::MatrixXd F; // 状态转移矩阵  
    Eigen::MatrixXd H; // 测量矩阵  
    Eigen::MatrixXd P; // 协方差矩阵  
    Eigen::MatrixXd Q; // 过程噪声矩阵  
    Eigen::MatrixXd R; // 测量噪声矩阵  
};
int main() {
    ExtendedKalmanFilter ekf;
    // 假设的objectPoints，这里只给出x和y坐标    
    std::vector<Eigen::Vector2d> objectPoints = {
        {117.9, 149.8},
        {120.8,163.4},
        {121.6, 172.8},
        {130.4, 166.5}
    };

    double delta_t = 0.1; // 时间间隔    

    // 确保EKF已经初始化  
    if (!ekf.IsInitialized()) {
        std::cerr << "EKF is not initialized!" << std::endl;
        return 1;
    }

    for (const auto& point : objectPoints) {
        // 预测（基于之前的运动模型）  
        ekf.Prediction(delta_t);

        // 假设objectPoints中的点就是测量值，进行测量更新  
        Eigen::VectorXd measurement = Eigen::VectorXd(2);
        measurement << point(0), point(1);

        // 更新EKF的状态（在真实应用中，这里会处理传感器数据）  
        Eigen::VectorXd updated_state = ekf.MeasurementUpdate(measurement);

        // 打印更新后的状态（x和y位置）  
        std::cout << "Updated position: (" << updated_state(0) << ", " << updated_state(1) << ")" << std::endl;
    }
    return 0;
}

在这个程序中输入了几个测试值，用来检测模型的准确性，输出结果还算满意。

Updated position: (117.43, 149.203)
Updated position: (118.843, 160.723)
Updated position: (119.679, 170.042)
Updated position: (128.311, 163.898)