c++ 卡尔曼滤波代码

以下是一个简单的卡尔曼滤波器的C ++实现：

#include <iostream>
#include <cmath>

using namespace std;

// 卡尔曼滤波器类
class KalmanFilter {
public:
  // 构造函数
  KalmanFilter(double init_x, double init_p, double init_q, double init_r) {
    x_ = init_x; // 初始状态估计
    p_ = init_p; // 初始协方差矩阵
    q_ = init_q; // 过程噪声协方差
    r_ = init_r; // 测量噪声协方差
  }

  // 卡尔曼滤波更新函数
  void update(double z, double dt) {
    // 预测更新
    double dt2 = dt * dt;
    double x_pred = x_;
    double p_pred = p_ + q_ * dt2;
    // 测量更新
    double k = p_pred / (p_pred + r_);
    x_ = x_pred + k * (z - x_pred);
    p_ = (1 - k) * p_pred;
  }

  // 获取当前状态估计
  double getState() {
    return x_;
  }

private:
  double x_; // 系统状态估计
  double p_; // 系统状态协方差矩阵
  double q_; // 过程噪声协方差
  double r_; // 测量噪声协方差
};

int main() {
  // 创建一个卡尔曼滤波器对象
  KalmanFilter kf(0, 1, 0.01, 0.1);

  // 模拟一些测量值
  double measurements[10] = { 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 };
  double dt = 0.1;

  // 使用卡尔曼滤波器更新状态估计
  for (int i = 0; i < 10; i++) {
    kf.update(measurements[i], dt);
    cout << "x[" << i << "] = " << kf.getState() << endl;
  }

  return 0;
}

在这个示例中，我们创建了一个KalmanFilter类，它包含了卡尔曼滤波器的状态变量和更新函数。使用该类对象，我们模拟了一些测量值并更新了状态估计。最后，我们打印出了每个时间步的状态估计值。