卡尔曼滤波

算法的核心思想是：根据当前的仪器”测量值” 和上一时刻的 “预测量” 和 “误差”,计算得到当前的最优量，再预测下一时刻的量。
其中突出的是观点是：把误差纳入计算, 而且分为预测误差和测量误差两种，统称为噪声
一个重要特点是：误差独立存在, 始终不受测量数据的影响
**

一个容易理解的例子：


k时刻预测值=k-1时刻最优值
测量值时刻更新

计算均方差 ，估算出k时刻最优值=k+1时刻预测值，同时要计算k时刻最优值的偏差

五个方程：（多维数据需要写成矩阵的形式）

了解KF算法的优势：
（1）可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
（2）在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。

对照下面的解释 理解参数含义：

下面分别解释五个方程各个参数的含义

1.预测状态方程

（1）方程

（2）备注

  ① X k-1|k-1 为k-1时刻的输出。
   ②当X为一维数据时，Fk的值是1。
   ③一维数据下（uk=0时）：系统预测值 = 系统状态变量k-1时刻的最优值。
**

2. 预测协方差方程

（1）目的

  根据 k-1时刻的系统协方差 预测 k时刻系统协方差。

（2）方程

（3）备注

  ①当X为一维数据时，Fk的值是1（同上）

3. 卡尔曼增益方程

（1）目的

  根据（k时刻）协方差矩阵的预测值 计算 卡尔曼增益。

（2）方程

（3）备注

  ①. 当 Pk|k-1 为一个一维矩阵时，Hk 是1。

4. 跟新最优值方程（卡尔曼滤波的输出）

（1）目的

  根据 状态变量的预测值 和 系统测量值 计算出 k时刻状态变量的最优值。

（2）方程

（3）备注

  ①当 Pk|k-1 为一个一维矩阵时，Hk 是1

5. 更新协方差方程

（1）目的

  为了求 k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）

（2）方程

（3）备注

  ①当 Pk|k-1 为一个一维矩阵时，Hk 是1。

简单的一维数据滤波的程序：
//1. 结构体类型定义
typedef struct
{
float LastP;//上次估算协方差 初始化值为0.02
float Now_P;//当前估算协方差 初始化值为0
float out;//卡尔曼滤波器输出 初始化值为0
float Kg;//卡尔曼增益 初始化值为0
float Q;//过程噪声协方差 初始化值为0.001 （对应于上面ppt中自己预测的不确定度）
float R;//观测噪声协方差 初始化值为0.543
}KFP;//Kalman Filter parameter

//2. 以角度为例 定义卡尔曼结构体并初始化参数
KFP KFP_angle={0.02,0,0,0,0.001,0.543};
int kalman_angle=0;
/**
*卡尔曼滤波器
@param KFP kfp 卡尔曼结构体参数
*float input 需要滤波的参数的测量值（即传感器的采集值）
*@return 滤波后的参数（最优值）
*/
float kalmanFilter(KFP *kfp,float input)
{**
//②预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
** 注： 这里的协方差对应偏差的平方值
kfp->Now_P = kfp->LastP + kfp->Q;
//③卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);
//④更新最优值方程：k时刻状态变量的最优值 = 预测值 + 卡尔曼增益 * （测量值 - 预测值）
注： **这一次的预测值就是上一次的输出值
kfp->out = kfp->out + kfp->Kg * (input - kfp->out);
//⑤更新协方差方程:
kfp->LastP = (1-kfp->Kg) * kfp->Now_P;
return kfp->out;
}
kalman_angle = **kalmanFilter(&KFP_angle,测量值)；//调用滤波器**