C语言实现简单卡尔曼滤波

article/2025/10/27 22:36:45

https://www.bilibili.com/video/BV1ez4y1X7eR

DR.CAN讲的真的很好

卡尔曼滤波的步骤

步骤	说明
Step 1	计算卡尔曼增益
Step 2	更新本次迭代的估计值
Step 3	更新本次迭代的估计误差

具体请看上面DR.CAN的视频

代码

参数	说明
x_mea	测量值
x_est	估计值
e_mea	固有的测量误差，取决于测量工具的精度，假设测量工具量程是2000/%2，测量误差就是2000*2%=40
e_est	估计误差，每次进行估计后需要更新
Kk	卡尔曼增益

大致过程：设定初始值启动卡尔曼滤波，启动完成后开始迭代，代码中初始化与第一个迭代就是卡尔曼滤波的启动过程

#include <stdio.h>
#include <stdlib.h>
#include <time.h>#define Kk_calc(x,y)    (x)/(x+y)struct KalmanFilter {float x_mea; // measure value, instead of random numberfloat x_est; // estimate valuefloat e_mea; // measure offset, can not be removedfloat e_est; // estimate offsetfloat Kk; // Karlman Gain
};float RandomNumGenerator(int base, int range)
{float k = 0.0;float randomNum = 0.0;k = 2 * range * 10;randomNum = rand() % (int)k;k = base - range + (randomNum / 10);return k;
}void BoostRandomNumGenerator() {srand((unsigned)time(NULL));
}void Kalman_Init(KalmanFilter* kalmanFilter, float FirstMeaValue, float E_mea, float FirstEstValue, float E_est) {kalmanFilter->x_est = FirstEstValue;kalmanFilter->x_mea = FirstMeaValue;kalmanFilter->e_est = E_est;kalmanFilter->e_mea = E_mea;kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);
}void Kalman_Update(KalmanFilter* kalmanFilter, float newMeaValue) {float temp = kalmanFilter->e_est;kalmanFilter->x_est = kalmanFilter->x_est + kalmanFilter->Kk * (newMeaValue - kalmanFilter->x_est);kalmanFilter->x_mea = newMeaValue;kalmanFilter->Kk = Kk_calc(kalmanFilter->e_est, kalmanFilter->e_mea);kalmanFilter->e_est = (1 - kalmanFilter->Kk) * temp;
}int main()
{KalmanFilter k;BoostRandomNumGenerator();Kalman_Init(&k, 51.0, 3.0, 40, 5);for (int i = 0; i < 10; i++){// Ten iterationsKalman_Update(&k, RandomNumGenerator(50, 3));printf("%.3f | %.3f\n",k.x_mea,k.x_est);}return 0;
}