C++卡尔曼滤波代码

C++中的卡尔曼滤波代码可以用于估计系统状态，根据观测数据和系统模型进行状态预测和更新。以下是一个简单的C++卡尔曼滤波代码示例：

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

int main() {
    // 定义系统模型
    Mat A = (Mat_<float>(2, 2) << 1, 1, 0, 1); // 状态转移矩阵
    Mat H = (Mat_<float>(1, 2) << 1, 0); // 观测矩阵

    // 定义过程噪声和观测噪声协方差矩阵
    Mat Q = (Mat_<float>(2, 2) << 0.1, 0, 0, 0.1); // 过程噪声协方差矩阵
    Mat R = (Mat_<float>(1, 1) << 1); // 观测噪声协方差矩阵

    // 初始化状态和协方差矩阵
    Mat x = (Mat_<float>(2, 1) << 0, 0); // 初始状态
    Mat P = (Mat_<float>(2, 2) << 1, 0, 0, 1); // 初始协方差矩阵

    // 模拟观测数据
    float trueValue = 5.0; // 真实值
    float noise = 1.0; // 噪声
    float observedValue = trueValue + noise; // 观测值

    // 卡尔曼滤波
    Mat y;
    for (int i = 0; i < 100; i++) {
        // 预测
        x = A * x;
        P = A * P * A.t() + Q;

        // 更新
        y = Mat(observedValue) - H * x;
        Mat S = H * P * H.t() + R;
        Mat K = P * H.t() * S.inv();
        x = x + K * y;
        P = (Mat::eye(2, 2, CV_32F) - K * H) * P;

        // 输出估计值
        cout << "Estimated value: " << x.at<float>(0) << endl;

        // 更新观测值
        observedValue = trueValue + noise;
    }

    return 0;
}

这段代码使用了OpenCV库来进行矩阵运算和数据处理。它实现了一个简单的一维卡尔曼滤波器，用于估计一个随时间变化的真实值。在每次迭代中，通过预测和更新步骤，根据观测值来更新状态估计值，并输出估计结果。