扩展卡尔曼滤波(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)