关于卡尔曼滤波的C语言算法

卡尔曼滤波

 

看公式1，ECR是估计变化比，也就是一个系统参数，需要我们根据实际情况调的。因为我们只有一个温度传感器，假如是测室温，那么我们认为短时间内室温是恒定的，所以这个变化比就等一。

将上面的公式转为汉字可以理解为：

公式1：

	本次估计值 = 估计变化比 * 上次估计值 + 卡尔曼增益 * (测量值 - 上次估计值)

公式2：
	
	卡尔曼增益 = 根号下(当前估计协方差的平方 / (当前估计协方差的平方 + 当前测量测方差的平方))

公式3：
	
	下次估计协方差 = [根号下(1 - 卡尔曼增益)] * 当前估计协方差

公式4：

	下次测量协方差 = [根号下(1 - 卡尔曼增益)] * 当前测量协方差




算法步骤：

1、初始化：

	上次估计值 = 0
	当前估计协方差 = 0.1 （任意不为0的值）
	当前测量协方差 = 0.2 （任意不为0初值）

2、计算

	1、获得测量值（也就是温度传感器测到的温度）
	2、计算卡尔曼增益（公式二）
	3、计算当前估计值，并将当前估计值赋值给上次估计变量，用于下次使用
	4、利用公式三和四，计算下次估计协方差和下次测量协方差
	5、更新协方差： 下次估计协方差 = 当前估计协方差，下次测量协方差 = 当前估计协方差
	6、返回本次估计值，也就是返回本次滤波之后的温度

 

typedef struct
{
    float lastTimePredVal;            // 上次估计值
    float lastTimePredCovVal;         // 上次估计协方差
    float lastTimeRealCovVal;         // 上次实际协方差
    float kg;
}kalman_typedef;

void kalman_init(kalman_typedef* kalman)
{
	kalman->lastTimePredVal = 0;
	kalman->lastTimePredCovVal = 0.1;
	kalman->lastTimeRealCovVal = 0.1;
	kalman->kg = 0;
}

// val: 本次测量值
float kalman_calc(kalman_typedef* kalman, float val)
{
    float currPredVal = 0;                             // 本次估计值
    float currRealVal = val;                           // 本次实际值
    float currPredCovVal = kalman->lastTimePredCovVal;         // 本次估计协方差值
    float currRealCovVal = kalman->lastTimeRealCovVal;         // 本次实际协方差值

    // 计算本次估计值，并更新保留上次预测值的变量
    currPredVal = kalman->lastTimePredVal + kalman->kg * (currRealVal - kalman->lastTimePredVal);
    kalman->lastTimePredVal = currPredVal;

    //计算卡尔曼增益
    kalman->kg = sqrt(pow(kalman->lastTimePredCovVal, 2) / (pow(kalman->lastTimePredCovVal, 2) + pow(kalman->lastTimeRealCovVal, 2)));

    // 计算下次估计和实际协方差
    kalman->lastTimePredCovVal = sqrt(1.0 - kalman->kg) * currPredCovVal;
    kalman->lastTimeRealCovVal = sqrt(1.0 - kalman->kg) * currRealCovVal;

    // 返回本次的估计值,也就是滤波输出值
    return currPredVal;
}

---------------------------------------------------------------------------------------------------------------------------------

另外一段代码：

卡尔曼滤波的步骤

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

代码

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

 

#include 
#include 
#include 

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