kalman滤波（一）

一.引言

1.卡尔曼滤波中的真实值,测量值,预测值,估计值怎么区分?
他的5条公式是其核心内容，结合现代的计算机,其实卡尔曼的程序相当的简单,只要你理解了他的那5条公式.

用一个简单的小例子：假设我们要研究的对象是一个房间的温度。根据你的经验判断，这个房间的温度是恒定的， 也就是现在这一分钟的温度等于过去一分钟的温度（假设我们用一分钟来做时间单位）（先验估计） 。假设你对你的经验不是 100% 的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分布。另外，我们在房间里放一个温度计，但是这个温度计也不准确的， 测量值和实际值有偏差，我们也把这些偏差看成是高斯白噪声。好了， 现在对于某一分钟我们有两个有关于该房间的温度值： 你根据经验的预测值 （系统的 预测值）和温度计的值（测量值）。

Kalman要解决的问题是如何使用这两个值结合他们各自的噪声来估算出房间的实际温度值。
假如我们要估算k时刻的实际温度值.首先你要根据k-1时刻的温度值,来预测k时刻的温度.因为你相信温度是恒定的,所以你会得到k时刻的温度预测值是跟k-1时刻一样的,假定是23度,同时该值的高斯噪声的偏差是5度[5是这样得到的：如果k-1时刻最优估计误差为3,你对自己预测噪声标准差是4度,他们平方和再开方,就是5）.至于为何是平方和，可以看做两个高斯过程相加[上次最优估计结果是个高斯过程，这次预测也是高斯过程]，所得的也是高斯过程，方差为原先两者的方差之和）]

然后，你从温度计那里得到了 k 时刻的温度值，假设是 25 度，同时该值的噪声标准差是 4 度。 由于我们用于估算 k 时刻的实际温度有两个温度值， 分别是 23 度和 25 度。 究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点， 我们可以上次的估计值的噪声方差及上次的最优估计方差总和之比判断。算出比例因子Kg: Kg^2=5^2/(5^2+4^2) ，所以 Kg=0.78 ，我们可以估算出 k 时刻的实际温度值（最优估计）是： 23+0.78*(25-23)=24.56 度[估计值+Kg*(测量值-估计值)].可以看出,因为温度计的协方差比较小（比较相信温度计）,所以估算出的最优温度值偏向温度计的值.

现在我们已经得到 k 时刻的最优温度值了，下一步就是要进入 k+1 时刻，进行新的最优估算。到现在为止，好像还没看到什么递归的东西出现。对了，在进入 k+1 时刻之前，我们还要算出 k 时刻那个最优值（ 24.56 度）的噪声标准差。算法如下： ((1-Kg)*5^2)^0.5=2.35 。这里 的 5 就是上面的k 时刻你预测的那个 23 度温度值的标准差，得出的 2.35 就是进入 k+1 时刻以 后 k 时刻估算出的最优温度值的标准（对应于上面的 3 ） 。 就是这样，卡尔曼滤波器就不断递归，从而估算出最优的温度值。他运行的 很快， 而且它只保留了上一时刻的 最优估计误差标准差。 上面的Kg ， 就是卡尔曼增益 （ Kalman Gain ） 。 他可以随不同的时刻而改变他自己的值，是不是很神奇！

2.卡尔曼滤波递归过程

卡尔曼滤波器涉及一些基本的概念知识,包括概率（Probability）,随机变量（Random Variable）,高斯或正态分配（Gaussian Distribution）等.

首先,要引入一个离散控制过程的系统：该系统可用一个线性随机微分方程来描述：X(k)=A X(k-1)+B U(k)+W(k)再加上系统的测量值：Z(k)=H X(k)+V(k)上两式子中,X(k)是k时刻的系统状态,U(k)是k时刻对系统的控制量.A和B是系统参数,对于多模型系统,他们为矩阵.Z(k)是k时刻的测量值,H是测量系统的参数,对于多测量系统,H为矩阵.[常见的：A为状态转移矩阵;H为观测矩阵].W(k)和V(k)分别表示过程和测量的噪声.他们被假设成高斯白噪声,他们的协方差（covariance）分别是Q,R（这里我们假设他们不随系统状态变化而变化）.
过程：对于满足上面的条件(线性随机微分系统,过程和测量都是高斯白噪声),卡尔曼滤波器是最优的信息处理器.首先我们要利用系统的过程模型,来预测下一状态的系统.假设现在的系统状态是k,根据系统的模型,可以基于系统的上一状态而预测出现在状态：X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)
式(1)中,X(k|k-1)是利用上一状态预测的结果,X(k-1|k-1)是上一状态最优的结果,U(k)为现在状态的控制量,如果没有控制量,它可以为0.到现在为止,我们的系统结果已经更新了,可是,对应于X(k|k-1)的covariance还没更新.我们用P表示covariance：P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
式(2)中,P(k|k-1)是X(k|k-1)对应的covariance,P(k-1|k-1)是X(k-1|k-1)对应的covariance,A’表示A的转置矩阵,Q是系统过程的covariance.式子1,2就是卡尔曼滤波器5个公式当中的前两个,也就是对系统的预测.现在我们有了现在状态的预测结果,然后我们再收集现在状态的测量值.结合预测值和测量值,我们可以得到现在状态(k)的最优化估算值X(k|k)：X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
到现在为止,我们已经得到了k状态下最优的估算值X(k|k).但是为了要另卡尔曼滤波器不断的运行下去直到系统过程结束,我们还要更新k状态下X(k|k)的covariance：P(k|k)=（I-Kg(k) H）P(k|k-1)………(5)
其中I为1的矩阵,对于单模型单测量,I=1.当系统进入k+1状态时,P(k|k)就是式子(2)的P(k-1|k-1).这样,算法就可以自回归的运算下去.

卡尔曼滤波器的原理基本描述了,根据这5个公式,可以很容易的实现计算机的程序.

最优估计的协方差（误差协方差）：为何要称作协方差呢？协方差是两个变量间的总体误差，P(k|k-1)可以理解为这是上一次最优估计和当前预测值(从时间上看是两个变量)的协方差。

 

3.卡尔曼滤波器的工作过程：

把房间看成一个系统,然后对这个系统建模:

运动方程：X(k)=A X(k-1)+B U(k)+W(k)

观测方程：Z(k)=H X(k)+V(k)

房间的温度是跟前一时刻的温度相同的,所以A=1.没有控制量,所以U(k)=0.因此得出：X(k|k-1)=X(k-1|k-1) ……….. (6)
式子（2）可以改成：P(k|k-1)=P(k-1|k-1) +Q ……… (7)
因为测量的值是温度计的,跟温度直接对应,所以H=1.式子3,4,5可以改成以下：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1)) ……… (8)
Kg(k)= P(k|k-1) / (P(k|k-1) + R) ………
P(k|k)=（1-Kg(k)）P(k|k-1)………(10)
现在模拟一组测量值作为输入.假设房间的真实温度为25度,模拟200个测量值,这些测量值的平均值为25度,但是加入了标准偏差为几度的高斯白噪声.为了令卡尔曼滤波器开始工作,需要告诉卡尔曼两个零时刻的初始值,是X(0|0)和P(0|0).他们的值不用太在意,随便给一个就可以了,因为随着卡尔曼的工作,X会逐渐的收敛.但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛.选取X(0|0)=1度,P(0|0)=10.

二.Kalman 的物理意义

1.首先确定几个符号（十分重要）：

2.几个假设和模型 

      N时刻状态量： X[n]=A*X[n-1|n-1]+B*U(n)+W[n]

即等于上个时刻的“真实值”(最优估计值)+控制量+误差（W[n]~N(0,Q)）。   因此，给出先验估计X[n|n-1]= A*X[n-1|n-1]+B*U(n);

      N时刻的观测值：Z[n]=H*X[n]+V[n]

      即等于真实值得数乘+误差（V[n]~N(0,R)）

利用预测值和测量值的残差来修正估计：X[n|n]=X[n|n-1]+K[n]*Residul[n]。

所以问题只剩下如何找到合适的K[n]使得估计最优，即后验误差协方差P[n|n]最小(越小说明越接近真实值X[n])

3.Kalman 过程详解：

(1) 预测：做出先验估计x[n|n-1]=A*x[n-1|n-1];

【对于一维的情况，A可以看成一个常数使用，经常取1，同时对于B经常取零(---可能有人会有疑问：取0没事吗，可以放心的告诉你，问题不大。反过来想想，这只是一个估计，可以在估计噪声方差得到修正)】

(2) 预测误差协方差：做出预测后的新的概率分布的方差（预测上次的最优估计为当前时刻的先验估计这个过程可以当成一个符合预测过程噪声分布的和另一个(上一次的最优估计可以看做高斯分布的)也符合高斯分布的相加。预测结果也是符合高斯噪声分布的，方差是两个相互独立的方差之和）。

【对于一维的情况,P[n|n-1]=P[n-1|n-1]+Q。 Q为预测方差，代表对预测的不信任程度，工程上根据实际调节以改善滤波器的性能:动态效果和去噪效果】

(3) 计算卡尔曼增益：

【对于一维的情况，K[n]=H*P[n|n-1]/{H^2*P[n|n-1]+R}。其中H是对观测的响应倍数，通常取1，R为测量的方差，工程上一般都可以直接获得】

(4) 得估计值：做出后验估计，修正后的估计值，更接近真实值。

      【对于一维的情况，最优估计由下式给出：x[n|n]=x[n|n-1]+K[n]*{z[n]-x[n|n-1]}。其中z[n]为观测值】

(5) 更新误差协方差：得到最优估计的概率分布的方差。

【对于一维的情况，新的误差协方差由下式给出：P[n|n]=(1-K[n]*H)*P[n|n-1]】

代码：

#include "Kalman.h"
/**
 *@function: - 卡尔曼滤波器初始化
 *@kalmanFilter：卡尔曼滤波器结构体
 *@init_x：待测量的初始值
 *@init_p：后验状态估计值误差协方差的初始值
 */
void kalmanFilter_init(KalmanStructTypedef *kalmanFilter, float init_x, float init_p,float predict_q,float newMeasured_q)
{
    kalmanFilter->x = init_x;//待测量的初始值，如有中值一般设成中值
    kalmanFilter->p = init_p;//后验状态估计值误差协方差的初始值（不要为0问题不大）
    kalmanFilter->A = 1;
    kalmanFilter->H = 1;
    kalmanFilter->q = predict_q;//预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
    kalmanFilter->r = newMeasured_q;//测量（观测）噪声方差R，可以通过实验手段获得
}

/**
 *@function: - 卡尔曼滤波器
 *@kalmanFilter:卡尔曼结构体
 *@newMeasured；测量值
 *返回滤波后的值
 */
float kalmanFilter_filter(KalmanStructTypedef *kalmanFilter, float newMeasured)
{
    /* Predict */
    kalmanFilter->x = kalmanFilter->A * kalmanFilter->x;//%x的先验估计由上一个时间点的后验估计值和输入信息给出
    kalmanFilter->p = kalmanFilter->A * kalmanFilter->A * kalmanFilter->p + kalmanFilter->q;  /*计算先验均方差 p(n|n-1)=A^2*p(n-1|n-1)+q */

    /* Correct */
    kalmanFilter->gain = kalmanFilter->p * kalmanFilter->H / (kalmanFilter->p * kalmanFilter->H * kalmanFilter->H + kalmanFilter->r);
    kalmanFilter->x = kalmanFilter->x + kalmanFilter->gain * (newMeasured - kalmanFilter->H * kalmanFilter->x);//利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出
    kalmanFilter->p = (1 - kalmanFilter->gain * kalmanFilter->H) * kalmanFilter->p;//%计算后验均方差

    return kalmanFilter->x;//得到现时刻的最优估计
}

#ifndef _Kalman_H_
#define _Kalman_H_
//标量卡尔曼滤波
typedef struct {
    float x;  // 系统的状态量
    float A;  // x(n)=A*x(n-1)+u(n),u(n)~N(0,q)
    float H;  // z(n)=H*x(n)+w(n),w(n)~N(0,r)
    float q;  // 预测过程噪声协方差
    float r;  // 测量过程噪声协方差
    float p;  // 估计误差协方差
    float gain;//卡尔曼增益
}KalmanStructTypedef;
void kalmanFilter_init(KalmanStructTypedef *kalmanFilter, float init_x, float init_p,float predict_q,float newMeasured_q);
float kalmanFilter_filter(KalmanStructTypedef *kalmanFilter, float newMeasured);
#endif