扩展卡尔曼滤波算法（C语言代码）

扩展卡尔曼滤波（Extended Kalman Filter, EKF）是一种常用的状态估计方法，适用于非线性系统。在 C++ 中实现 EKF，可以通过类和函数的方式更好地组织代码。下面是一个简单的 C++ 实现示例，涵盖了 EKF 的基本步骤：状态预测、协方差预测、状态更新和协方差更新。

1. 头文件和类定义

#include <iostream>
#include <vector>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

class ExtendedKalmanFilter {
public:
    ExtendedKalmanFilter(int state_dim, int measurement_dim);

    void predict();
    void update(const VectorXd& z);

    VectorXd getState() const { return x_; }
    MatrixXd getCovariance() const { return P_; }

private:
    int state_dim_; // 状态维度
    int measurement_dim_; // 测量维度

    VectorXd x_; // 状态向量
    MatrixXd P_; // 协方差矩阵

    MatrixXd F_; // 状态转移矩阵
    MatrixXd H_; // 测量矩阵
    MatrixXd Q_; // 过程噪声协方差矩阵
    MatrixXd R_; // 测量噪声协方差矩阵

    MatrixXd I_; // 单位矩阵
};

// 构造函数
ExtendedKalmanFilter::ExtendedKalmanFilter(int state_dim, int measurement_dim)