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

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

DR.CAN讲的真的很好

卡尔曼滤波的步骤

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

代码

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

#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
",k.x_mea,k.x_est);
	}

	return 0;
}

运行效果