卡尔曼滤波

一、概述

所谓的滤波即滤除噪声。

图中红色线是传感器传入的数据，由于存在噪声，使得波动很大。在滤除噪声后，数据如紫色线条所示。

1.1适用的系统

卡尔曼滤波适用于线性高斯系统。所谓的线性，指的是系统满足叠加性和其次性。所谓的高斯，指的是系统噪声为高斯噪声。
卡尔曼滤波可以视为一种加权，即选出预测值、测量值的权重（卡尔曼增益）。

1.2状态空间表达

由状态方程、观测方程组成

1.2.1状态方程

其中:

xk：系统k时刻的状态
uk-1：系统k-1时刻输入的控制量
wk-1：系统k-1时刻的状态噪声，白噪声（w~N(0,Q)）

• 状态方程的主要作用为：根据k-1时刻的后验，确定k时刻的先验，同时更新k时刻先验状态的协方差矩阵

1.2.2观测方程

其中:

zk：系统k时刻的观测量，即传感器获得的带有噪声的数据
H：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
比如：状态量为：位移s，速度v；观测量为位移s，则H为1*2矩阵
vk：观测的噪声，服从高斯分布，vk~N(0,R)。

• 观测方程的主要作用为：获得H矩阵，为后面的修正做准备。
  
  在上图中，横轴为机器人的位姿，纵轴为机器人在该位置处的概率。
  黑色的曲线：k-1时刻，机器人的位置状态分布。
  黄色的曲线：k时刻，机器人的先验位置状态分布
  紫色的曲线：k时刻，机器人观测到的位置状态分布
  红色的曲线：k时刻，融合后的机器人位置状态分布

二、卡尔曼滤波公式

卡尔曼滤波公式由两部分组成：预测、校正

2.1预测公式

根据k-1时刻的状态去估计k时刻的状态，获得k时刻状态的先验信息。
计算k时刻状态先验信息的协方差矩阵
根据运动方程获得

2.2更新公式

计算卡尔曼增益：卡尔曼增益决定了，预测状态、观测状态的可靠程度。如果预测状态可靠的话，卡尔曼增益小。如果观测状态可靠的话，卡尔曼增益大。
更新当前状态信息
更新当前状态的协方差矩阵

三、卡尔曼滤波流程

1. 确定状态量、观测量
2. 构建方程（运动方程----先验估计----先验状态协方差矩阵----观测方程----后验估计----后验状态协方差矩阵）
3. 确定初识估计值： Q、R、x0（初识状态，一般为0）、p0（初始状态协方差矩阵）
4. 迭代
5. 调节Q、R

示例

1. 选择状态变量、观测变量
   状态变量：高度h
   观测变量：高度h

2. 构建方程
   

3. 确定初始值
   x0 = 0 p0=1 R=0.01

4. 迭代

#include <iostream>
#include <vector>
using namespace std;
int main()
{
    // 初识变量
    long double x0 = 0;           // 状态初始值
    long double R = 0.01;      // 观测方差
    long double p0 = 1;           // 初识状态协方差矩阵
    long double status[10] = {20,25,31,31,25,30,35,28,24};  // 观测数据
    long double x = x0;                // 状态信息
    long double p = p0;                // 状态的协方差矩阵
    long double  K;                    // 卡尔曼增益
    vector<double> vn_con;                // 存储滤波后的结果
    for(int i = 0;i<10;i++)
    {
        // 1、根据运动方程获得先验状态信息
        x = x;
        // 2、计算先验状态协方差矩阵
        p = p;
        // 3、计算卡尔曼增益
        K = (p)/(p+R);
        // 4、根据观测、预测进行后验状态估计
        x = x + K*(status[i]-x);
        // 5、更新后验协方差矩阵
        p = (1 - K)*p;
        vn_con.push_back(x);
    }
    for(double res:vn_con)
        cout<<res<<"  ";
    cout<<endl;
}

四、滤波结果可视化

绘制上例的折线图

可见滤波后的折线图明显平稳很多。