卡尔曼滤波器

// 导入Qt的头文件
#include <QtCore/QCoreApplication>
#include <QDebug>

// 定义卡尔曼滤波器类
class KalmanFilter {
public:
    KalmanFilter(double initValue, double initProcessNoise, double initMeasurementNoise) {
        x = initValue;
        p = initProcessNoise;
        q = initMeasurementNoise;
    }

    double update(double measurement) {
        // 预测
        double x_pred = x;
        double p_pred = p + q;

        // 更新
        double k = p_pred / (p_pred + q);
        x = x_pred + k * (measurement - x_pred);
        p = (1 - k) * p_pred;

        return x;
    }

private:
    double x; // 状态变量
    double p; // 状态协方差
    double q; // 测量协方差
};

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    // 初始化卡尔曼滤波器
    KalmanFilter kf(0, 1, 1);

    // 模拟测量值
    double measurements[] = {1, 2, 3, 4, 5};

    // 使用卡尔曼滤波器进行滤波
    for (int i = 0; i < 5; i++) {
        double filteredValue = kf.update(measurements[i]);
        qDebug() << "Measurement:" << measurements[i] << "Filtered:" << filteredValue;
    }

    return a.exec();
}