#include <stdio.h>
#include <stdlib.h>
// 简化的卡尔曼滤波器结构体
typedef struct {
double x; // 状态估计
double P; // 状态协方差
double Q; // 过程噪声协方差
double R; // 测量噪声协方差
} KalmanFilter;
// 预测步骤
void predict(KalmanFilter *kf) {
kf->x = kf->x; // 状态预测（此处简化为不变）
kf->P = kf->P + kf->Q; // 更新过程噪声协方差
}
// 更新步骤
void update(KalmanFilter *kf, double z) {
double K = kf->P / (kf->P + kf->R); // 计算卡尔曼增益
kf->x = kf->x + K * (z - kf->x); // 更新状态估计
kf->P = (1 - K) * kf->P; // 更新状态协方差
}
int main() {
KalmanFilter kf = {0, 1, 0.1, 1}; // 初始化卡尔曼滤波器
double measurements[] = {1, 2, 3, 4, 5}; // 测量值数组
int n = sizeof(measurements) / sizeof(measurements[0]);
for (int i = 0; i < n; i++) {
predict(&kf); // 预测步骤
update(&kf, measurements[i]); // 更新步骤
printf("Estimate: %.2f\n", kf.x); // 打印状态估计
}
return 0;
}
|
楼主
