卡尔曼滤波算法及C语言实现
#include <stdio.h>
// 卡尔曼滤波器结构体
typedef struct {
float x; // 状态变量
float p; // 状态误差协方差矩阵
float q; // 过程噪声协方差
float r; // 测量噪声协方差
float h; // 测量矩阵
float k; // 卡尔曼增益
float y; // 测量值
} KalmanFilter;
// 卡尔曼滤波器更新函数
void kalmanFilterUpdate(KalmanFilter *kf, float measurement) {
kf->y = measurement - kf->h * kf->x; // 测量更新
kf->p = kf->h * kf->p * kf->h->transpose() + kf->r; // 预测误差更新
kf->k = kf->p * kf->h->transpose() / (kf->h * kf->p * kf->h->transpose() + 1.0f); // 卡尔曼增益计算
kf->x = kf->x + kf->k * kf->y; // 状态更新
kf->p = (1.0f - kf->k) * kf->p; // 预测误差新值
}
int main() {
KalmanFilter filter;
filter.x = 0.0f;
filter.p = 1.0f;
filter.q = 1e-5f;
filter.r = 1.0f;
filter.h = 1.0f;
float measurement = 1.0f; // 假设的测量值
kalmanFilterUpdate(&filter, measurement);
printf("状态变量: %f\n", filter.x);
printf("状态误差协方差矩阵: %f\n", filter.p);
return 0;
}
页:
[1]