- #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;
- }
| 
楼主
