卡尔曼滤波理论知识

优点

采用递归方法解决线性滤波问题，只需要当前的测量值和前一个采样周期的估计值就能够进行状态估计，不需要大量的存储空间，每一步的计算量小，计算步骤清晰，非常适合计算机处理。

卡尔曼方程式

预测状态方程

1. 预测状态方程是根据系统状态变量k-1时刻的最优值和系统输入计算出 k时刻系统预测值。

$$X_{k|k-1}=F_k{X}_{k-1|k-1}+B_k{u}_k$$

• $$X_{k|k-1}$$是利用k-1时刻预测的当前状态结果
• $$X_{k-1|k-1}$$是K-1时刻最优值
• $$F_k$$是作用在$$X_{k-1|k-1}$$状态下的变换矩阵，它是算法对状态变量进行预测的依据
• $$B_k$$是作用在控制量上的变换矩阵，在大多数实际情况下并没有控制增益
• $$u_k$$是当前状态的控制增益，一般没有这个变量，可以设为0

预测协方差方程

2. 预测协方差方程是根据k-1时刻系统协方差矩阵预测k时刻系统协方差矩阵

$$P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k$$

• $$P_{k|k-1}$$是k时刻系统协方差矩阵
• $$P_{k-1|k-1}$$是k-1时刻系统协方差矩阵(LastP)
• $$Q_k$$是系统过程噪声的协方差。(Q)
  • 协方差矩阵只要确定了一开始的$$P_0$$，后面都可以递推出来；
  • 而且初始协方差矩阵$$P_0$$，只要不是为0，它的取值对滤波效果影响很小，都能很快收敛，

卡尔曼增益方程

3. 卡尔曼增益方程是根据协方差矩阵预测值计算卡尔曼增益

$$K_k=\frac{P_{k|k-1}{H}_k^T}{H_k{P}_{k|k-1}{H}_k^T+R_k}$$

• $$K_k$$是卡尔曼增益，是滤波的中间结果(Kg)
• $$H_k$$是对象的预测矩阵，P是以为矩阵，H也是1
• $$R_k$$是对象测量噪声的协方差矩阵(R)，它是一个数值，作为已知条件输入滤波器。注意：
  • 这个值过大过小都会使滤波效果变差，
  • 且$$R_k$$取值越小收敛越快，所以可以通过实验手段寻找合适的$$R_k$$值再利用它进行真实的滤波。

更新最优值方程

4. 更新最优值方程是根据状态变量预测值和系统测量值计算出k时刻状态变量最优值

$$X_{k|k}=X_{k|k-1}+K_k(Z_k-H_k{X}_{k|k-1})$$

• $$X_{k|k}$$是k时刻状态变量最优估计值(out)
• $$Z_k$$是对象的测量值

##更新协方差方程

5. 更新协方差方程是求k时刻协方差矩阵方程

$$P_{k|k}=(I-K_k{H}_k)P_{k|k-1}$$

• $$P_{k|k}$$为k时刻协方差矩阵(Now_P)
• I为单位矩阵
• 作用：用于下一次卡尔曼滤波

卡尔曼滤波参数

序号	类型	名称	描述	初始值
1	float	LastP	上次估算协方差	0.02
2	float	Now_P	当前估算协方差	0
3	float	out	卡尔曼滤波器输出值（最优值）	0
4	float	Kg	卡尔曼增益	0
5	float	Q	过程噪声协方差	0.001
6	float	R	观测噪声协方差	0.543

卡尔曼滤波结构体描述

//卡尔曼滤波参数   filter过滤器
struct _1_ekf_filter
{
	float LastP;
	float Now_P;
	float out;
	float Kg;
	float Q;
	float R;
};

扩展知识

1960年，卡尔曼发表了他著名的用递归方法解决离散数据线性滤波问题的论文。从那以后，得益于数字计算技术的进步，卡尔曼滤波器已成为推广研究和应用的主题，尤其是在自主或协助导航领域。

卡尔曼滤波器由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大:它可以估计信号的过去和当前状态，甚至能即使并不知i确切性质。