https://www.bilibili/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 number
float x_est; // estimate value
float e_mea; // measure offset, can not be removed
float e_est; // estimate offset
float 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 iterations
Kalman_Update(&k, RandomNumGenerator(50, 3));
printf("%.3f | %.3f\n",k.x_mea,k.x_est);
}
return 0;
}
运行效果
更多推荐
C语言实现简单卡尔曼滤波
发布评论