// 导入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();
}
卡尔曼滤波器
最新推荐文章于 2024-08-30 16:00:44 发布