Kalman Filter学习笔记

Kalman Filter学习笔记

说在前面

由于传感器本身的特性，任何测量结果都有误差。因此需要在传感器测量结果的基础上，进行跟踪，以此来保证所观测物体的信息不会发生突变。这就需要用到经典跟踪算法——卡尔曼滤波器。

---

工作原理

卡尔曼滤波器就是根据上一时刻的状态，预测当时时刻的状态，将预测的状态与当时时刻的测量值进行加权，加权后的结果才认为是当前的实际状态，而不是单单只是当前的测量值。

---

卡尔曼滤波器理论

以下7个方程即为卡尔曼滤波器的理论描述：
预测 Predication:
$x^{\prime }=Fx+u$
$p^{\prime }=FP{F}^T+Q$
测量更新 Measurement update:
$y=z-H{x}^{\prime }$
$K=P^{\prime }{H}^T(H{P}^{\prime }{H}^T+R{)}^{-1}$
$x=x^{\prime }+Ky$
$P=(I-KH)P^{\prime }$

接下来以飞行器追踪小车为例，分析这7个方程的含义：
首先需要通过物理量对小车的状态进行描述，小车相当于二维空间中的点，需要x，y方向上的坐标，x，y方向上的速度，才能表示。这样的状态方程就有4个变量。

一、预测：

1、$x^{\prime }=Fx+u$

$x^{\prime }:$预测状态向量
$F:$状态转移矩阵
$u:$外部影响
$x:$状态向量
x = $$\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]\text{\hspace{1em}(1)}$$
假设当前小车运动为匀速运动，即加速度为0，加速度不会对预测造成影响，即:
$u=0$

2、$p^{\prime }=FP{F}^T+Q$

$P:$协方差矩阵，表示系统的不确定度
$F:$ 在1中提到的状态转移矩阵
$Q:$过程噪声,无法通过方程1表示的噪声
由于摄像头模块只能确定小车的位置，无法测量小车的速度，因此对于摄像头模块来说，其位置信息准确度较高，不确定度较低；速度信息准确度较低，不确定度较高。
因此可以这样确定P
P = $$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 1& 1\\ 0& 0& 100& 0\\ 0& 0& 0& 100\end{array}\right]$$
Q在工程上一般设为单位矩阵参与运算，即
Q = $$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

二、观测：

1、$y=z-H{x}^{\prime }$

该公式计算的是实际观测值z同预测值x’之间的差值y。
因为z是实际观测值，只有x，y坐标两项。
而x‘是预测值，存在x，y，v_x，v_y四项。
所以需要状态转移矩阵H来将预测值x’进行一个转换，使其能与实际观察值z进行运算。

2、$K=P^{\prime }{H}^T(H{P}^{\prime }{H}^T+R{)}^{-1}$

$R:$为测量噪声矩阵，这个表示测量值与真值之间的差值。
$K:$卡尔曼增益 即上一个公式 y的权值

三、状态量更新：

1、$x=x^{\prime }+Ky$

该公式完成了当前状态向量的更新，综合考虑了上一时刻的预测值和当前时刻的测量值

2、$P=(I-KH)P^{\prime }$

$I:$与状态向量同样维度的单位矩阵
该公式根据卡尔曼增益，更新了系统的不确定度P，用于下一个周期的计算

---

具体代码实现

#include 
   #include 
#include 

typedef struct
{
    float x;//当前状态量
    float p;//状态协方差
    float z;//实际测量值
    float Q;//过程噪声
    float R;//测量噪声
    float K;//卡尔曼增益

    float x_;//预测下一时刻状态量
    float p_;//下一时刻协方差

}Kalman_Filter;

Kalman_Filter Kal = {0,0.02,0,0.1,0.5,0,0,0};//参数需要调节
Kalman_Filter *Kq = &Kal;

float Kalman_Filter_1(Kalman_Filter *Kal)//一维卡尔曼滤波
{
    //预测
    Kal->x_ = Kal->x;
    Kal->p_ = Kal->p + Kal->Q;
    
    //卡尔曼增益
    Kal->K = Kal->p_/(Kal->p_ + Kal->R);

    //更新
    Kal->x = Kal->x_ + Kal->K*(Kal->z - Kal->x_);
    Kal->p = (1 - Kal->K)*Kal->p_;
    return Kal->x; 
}

void main()
{
    float z_real = 1;
    Kq->x = z_real + rand()%11*0.03;//第一次的状态量
    int i = 0;
    for(i = 0; i < 40; i++)
    {
        Kq->z =  z_real + rand()%11*0.03;//观测所得数据
        Kalman_Filter_1(Kq);
        printf("Kal:%f\n",Kq->x);
        printf("K:%f\n",Kq->K);
        printf("Kal - real:%f\n",Kq->x-z_real);
        printf("noKal - real:%f\n",Kq->z-z_real);
        printf("kal - noKAl:%f\n",Kq->x-Kq->z);
        printf("\n");
    }

}

---

参数的确定

举例而言，R固定，Q越大，代表越信任侧量值，Q无穷代表只用测量值；
反之，Q越小代表越信任模型预测值，Q为零则是只用模型预测。
参数的确定才是用好卡尔曼算法的根本。
未完待续…