卡尔曼滤波算法及C语言实现

[复制链接]

21|0

手机看帖

扫描二维码
随时随地手机跟帖

电梯直达

楼主

everyrobin| 中级工程师

楼主 | 2024-12-23 12:03 | 只看该作者回帖奖励

|倒序浏览 |阅读模式

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;

}