文章目录

• 前言
• 原理讲解
• 代码实现
• 结语

前言

卡尔曼滤波是一种居然预测的功能的滤波器，常常被用在各种传感器融入滤波，机器人定位等等场合。当初美国阿波罗登月飞船也是利用卡尔曼滤波来进行预测的。
卡尔曼滤波常常用在具有不确定信息的动态系统中来预测系统下一步的变化，即使伴随着各种干扰，卡尔曼滤波总是做出最真实的预测。
本文只讲解一阶卡尔曼滤波。

原理讲解

假如现在有这样一种情况，假设有一辆小车，它只在一条直线上做匀速直线运动。我们设x表示它的位移，小车速度为v。则在某一个时刻，它的位置为 x k x_k xk​，但是呢，这个值并不准确，它应该在 x k x_k xk​的附近，且服从期望为 x ( k − 1 ) x(k-1) x(k−1)，方差 p ( k − 1 ) p(k-1) p(k−1)，如图：
则 △ \triangle △T过后，我们预测它最可能在 x ( k ) = x ( k − 1 ) + v ∗ x(k)=x(k-1)+v* x(k)=x(k−1)+v∗ △ \triangle △T处。但它也可能受到干扰，比如一阵风吹来，影响了它的速度，我们称这一阵风为过程噪声 w k w_k wk​,假设 w k w_k wk​服从期望为0，方差为 q k q_k qk​的正态分布。则， x ( k ) x(k) x(k)的方差 p ( k ) = p ( k − 1 ) + q k p(k)=p(k-1)+q_k p(k)=p(k−1)+qk​
如图：

此时的 x ( k ) x(k) x(k)与 p ( k ) p(k) p(k)被称为先验估计。
由于我们不能信赖这个先验的值，所以我们需要对这个值与传感器的值进行一个融合预测，来得到一个更好的值。由于传感器的值也是有误差的，我们设传感器的值服从一个期望为 z k z_k zk​方差为 r k r_k rk​的正态分布，如图：

现在我们得到了传感器的正态分布与先验估计的正态分布。现在我们要利用这二个正态分布来得到一个更加准确的正态分布。这个正态分布我们可以取如图正态分布：

好了，现在我们就得到了卡尔曼滤波的预测值即后验估计 x ′ ( k ) x'(k) x′(k)了。只不过我们现在还没有准确的算出它。数学上告诉我们，这个正态分布就是二个正态分布相乘的结果，并给出了公式。这里我们就不讨论公式的推导，直接给出方法：
设有一正态分布期望为 μ 1 \mu_1 μ1​,方差为 σ 1 \sigma_1 σ1​;另一正态分布期望为 μ 2 \mu_2 μ2​,方差为 σ 2 \sigma_2 σ2​。二者相乘的结果为期望为 μ \mu μ,方差为 σ \sigma σ的正态分布。
先求出卡尔曼增益 k g = σ 1 2 σ 1 2 + σ 2 2 k_g=\frac{\sigma_1^2}{\sigma_1^2+\sigma_2^2} kg​=σ12​+σ22​σ12​​
则
μ = μ 1 + k g ( μ 2 − μ 1 ) \mu=\mu_1+k_g(\mu_2-\mu_1) μ=μ1​+kg​(μ2​−μ1​)
σ = σ 1 2 ( 1 − k g ) \sigma=\sigma_1^2(1-k_g) σ=σ12​(1−kg​)
利用这个公式，即可简单推出以上例子的最终结果：
k = p （ k ） p （ k ） + r k k=\frac{p（k）}{p（k）+r_k} k=p（k）+rk​p（k）​
x ′ ( k ) = x ( k ) + k ( z k − x ( k ) ) x'(k)=x(k)+k(z_k-x(k)) x′(k)=x(k)+k(zk​−x(k))
p ′ ( k ) = p （ k ） ( 1 − k ) p'(k)=p（k）(1-k) p′(k)=p（k）(1−k)
现在就得到了用于在程序中迭代的公式，将这些公式用程序实现，即可达到预测小车状态的效果。

代码实现

下面给出一种卡尔曼滤波器的实现：（不是上述例子，可用于传感器滤波）
.h文件

#ifndef _KALMAN_H
#define _KALMAN_H

#include "stdlib.h"

typedef struct {
    float X_last; //上一时刻的最优结果
    float X_mid;  //当前时刻的预测结果
    float X_now;  //当前时刻的最优结果
    float P_mid;  //当前时刻预测结果的协方差
    float P_now;  //当前时刻最优结果的协方差
    float P_last; //上一时刻最优结果的协方差
    float kg;     //kalman增益
    float A;      //系统参数
    float Q;
    float R;
    float H;
}kalman;

void kalmanCreate(kalman *p,float T_Q,float T_R);
float KalmanFilter(kalman* p,float dat);

#endif

.c文件

#include "kalman.h"


void kalmanCreate(kalman *p,float T_Q,float T_R)
{
    //kalman* p = ( kalman*)malloc(sizeof( kalman));
    p->X_last = (float)0;
    p->P_last = 0;
    p->Q = T_Q;
    p->R = T_R;
    p->A = 1;
    p->H = 1;
    p->X_mid = p->X_last;
    //return p;
}



float KalmanFilter(kalman* p,float dat)
{
    p->X_mid =p->A*p->X_last;                     //x(k|k-1) = AX(k-1|k-1)+BU(k)
    p->P_mid = p->A*p->P_last+p->Q;               //p(k|k-1) = Ap(k-1|k-1)A'+Q
    p->kg = p->P_mid/(p->P_mid+p->R);             //kg(k) = p(k|k-1)H'/(Hp(k|k-1)'+R)
    p->X_now = p->X_mid+p->kg*(dat-p->X_mid);     //x(k|k) = X(k|k-1)+kg(k)(Z(k)-HX(k|k-1))
    p->P_now = (1-p->kg)*p->P_mid;                //p(k|k) = (I-kg(k)H)P(k|k-1)
    p->P_last = p->P_now;                         //状态更新
    p->X_last = p->X_now;
    return p->X_now;
}

结语

对于多阶的卡尔曼滤波，涉及到状态空间，矩阵，协方差的概念，比较难以理解。下面给出具体原理讲解：卡尔曼滤波器。