关于卡尔曼滤波的理解

 结构体参数

---

    float Last_P;//上次估算协方差 
    float Now_P;//当前估算协方差
    float out;//卡尔曼滤波器输出
    float Kg;//卡尔曼增益
    float Q;//过程噪声协方差
    float R;//观测噪声协方差

struct _1_ekf_filter
{
	float LastP;
	float Now_P;
	float out1;
	float Kg;
	float Q;
	float R;	
};

extern void kalman_1(struct _1_ekf_filter *ekf,float input); 

 

各参数的整定

---

关键值P，它是误差协方差初始值，表示对当前预测状态的信任度，它越小说明越相信当前预测状态；它的值决定了初始收敛速度，一般开始设一个较小的值以便于获取较快的收敛速度。这个值只是影响初始收敛速度，lastp初始不为0即可.

Out为输出值  自动算出  数学公式为   Y(k)=H∗X(k)

Kg 为增益  不需要初始化  自动算出   K(k)=P(k)∗HT∗(H∗P(k)∗HT+R)−1

Q值为过程噪声，越小系统越容易收敛，对模型预测的值信任度越高；但是太小则容易发散；Q值越大对于预测的信任度就越低，而对测量值的信任度就变高；如果Q值无穷大，那么我们只信任测量值.

R值为测量噪声，太小太大都不一定合适。R太大，卡尔曼滤波响应会变慢，因为它对新测量的值的信任度降低；越小系统收敛越快，但过小则容易出现震荡.

我觉得测试时可以先将Q从小往大调整，将R从大往小调整；先固定一个值去调整另外一个值，看收敛速度与波形输出.

具体的数学过程可以参考这篇文章卡尔曼滤波（Kalman Filter）原理理解和测试_卡尔曼滤波会对参数进行反馈吗-CSDN博客

---

 

一阶卡尔曼滤波的代码化

---

.c文件如下（目前可能有错）

#include "kalman.h"
#include <math.h>



float KalmanFilter(Kalman *ekf,float input)
{
  ekf->Now_P = ekf->LastP + ekf->Q;
   //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
  
  ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
   //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）

  ekf->out1 = ekf->out1 + ekf->Kg * (input - ekf->out1);
   //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    
  ekf->LastP = (1-ekf->Kg) * ekf->Now_P ;
   //更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。

  return ekf->out1;
   //返回输出值
}

 

---

 .h文件如下

#ifndef _KALMAN_H
#define _KALMAN_H




struct _1_ekf_filter
{
	float LastP;
	float Now_P;
	float out1;
	float Kg;
	float Q;
	float R;	
};


extern void kalman_1(struct _1_ekf_filter *ekf,float input);  

#endif


//例如
//static struct _1_ekf_filter ekf[1] = {{0.1,0,0,0,0.007,0.3}};
//                                        LP P O K   Q    R


	

 沒有init函数是因为参数在需要的时候定义

MPU6050读取加速度进行一维卡尔曼滤波的应用

---

#define  Acc_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0X3B,buffer,6) 
#define  Gyro_Read() IIC_read_Bytes(MPU6050_ADDRESS, 0x43,&buffer[6],6) 


int16_t MpuOffset[6] = {0};

static volatile int16_t *pMpu = (int16_t *)&MPU6050;




void MpuGetData(void) 
{
	 uint8_t i;
     uint8_t buffer[12];

	  Acc_Read();
	  Gyro_Read();

		for(i=0;i<6;i++)
		{
			pMpu[i] = (((int16_t)buffer[i<<1] << 8) | buffer[(i<<1)+1])-MpuOffset[i];		
			if(i < 3) 
			{
				{
					static struct _1_ekf_filter ekf[3] = {{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543},{0.02,0,8192,0,0.001,0.543}};	
                    \\三组结构体初始值
					kalman_1(&ekf[i],(float)pMpu[i]); 
					pMpu[i] = (int16_t)ekf[i].out1;
				}
		}
		
	}
}