滤波学习理解----EKF（一）

最近回到slam方向了，所以有时间整理一下最近的收获。
最复杂也是最简单的模块----滤波

引入

那么滤波是什么呢？
滤波就是由于观测observation（OB）天生具备的误差和噪声。当有多个信号源观测相同事物时他们的观测值可能是不同的。就像一千个人同时看维纳斯并表述维纳斯所在的位置和姿态。那么这一千个人的表述可能大同小异，但是你无法准确获知维纳斯的真实位置和姿态。那么通过这一千个表述，我们可以对他们进行滤波操作，尽可能还原真实的位置和姿态。

正常的滤波可能会考虑到均值滤波。即将n个人按时间排序，每k个人为一组的表述做一个平均的输出，也就是n个数据，滤波后输出n-k+1个数据。这样对于每组来说，他们中过于激进或保守的表述会被平均掉，对应到数据处理中也就是噪声的波峰和波谷被平滑掉。但是相对的我们输出表述数量会随着滤波次数的增加而减少，对应到数据处理中也就是数据缺失，数据输出频率发生改变。这是我们不期望的。
上面的表述是一个抽象的概念，在实际的数据处理中，很多时候输入和输出数据并不是一维的，甚至输入数据和输出数据的维度也是不同的。因此这种维度的改变已经超过平滑滤波所能处理的范围。
所以针对位姿问题，处理方式上EKF显然是一个不错的选择。并且在很多领域EKF依然是第一选择。

原理

首先我们需要两个方程对要滤波的对象建立数学模型。这个数学模型可以被抽象为如下两个方程的形式。

数学模型

X i = F ∗ X i − 1 + B ∗ U + Q (1) X_{i}=F*X_{i-1}+B*U+Q\tag{1} Xi​=F∗Xi−1​+B∗U+Q(1)
Z i = H ∗ X i + R (2) Z_{i}=H*X_{i}+R\tag{2} Zi​=H∗Xi​+R(2)

其中X为目标状态真值，F为运动函数，U为控制增量，B为控制增量的系数，Q为系统误差。
Z为观测的测量值，H为观测函数，R为观测误差。

方程理解

需要注意的是，X和Z虽然为真值和观测值，但是他们既可以是一个数值，也可以是个n维的向量。因此，F和H虽然是函数，但是可以用矩阵的形式来表达一个函数。此处需要矩阵论的基础概念。明白矩阵中每个元素相当于是对应各个方程中，各个变量的函数中的系数这一概念。
除此之外，BU通常在使用中是被舍弃的，因为我们通常的使用目的是多个观测源获得某一目标真值。BU的使用场景通常是带有控制目的或者控制变量的场景。例如控制机器人行走或者运动过程中，存在控制移动增量。

方程中的Q与R

然后是Q和R两个误差。在日常认知概念中，一种真值对一种误差或者是多个误差（例如温度计的误差可能是空气湿度，风速，玻璃折射，观测人近视，观测的操作方式存在问题等等等），为什么此处能够抽象为两个误差呢。是因为此处是将造成误差分为两类。一类是系统本身就存在的误差，例如空气湿度和风速，玻璃折射，即读数（观测）完全正确时也避免不了的误差。还有一类是观测带来的误差，例如观测人近视，观测的操作方式存在问题。即观测的传感器精度特别高。但是由于观测人或方法带来的误差。这样分类是因为一个误差来自于系统本身，一个来自于观测本身。
综上，方程表述如下

运动方程：
X=FX+BU+Q
观测方程：
Z=HX+R

滤波模型

针对上述模型我们可以得到如下的滤波模型
预测模型：
x i ′ = F ∗ x i − 1 + u (3) x'_{i}=F*x_{i-1}+u\tag{3} xi′​=F∗xi−1​+u(3)
P i ′ = F ∗ P i − 1 ∗ F T + Q (4) P'_{i}=F*P_{i-1}*F^{T}+Q\tag{4} Pi′​=F∗Pi−1​∗FT+Q(4)
更新模型：
y = z − H ∗ x i ′ (5) y=z-H*x'_{i}\tag{5} y=z−H∗xi′​(5)
K = P i ′ ∗ H T ∗ ( H ∗ P i ′ ∗ H T + R ) (6) K=P'_{i}*H^{T}*(H*P'_{i}*H^{T}+R)\tag{6} K=Pi′​∗HT∗(H∗Pi′​∗HT+R)(6)
x i = x i ′ + K ∗ y (7) x_{i}=x'_{i}+K*y\tag{7} xi​=xi′​+K∗y(7)
P i = ( I − K ∗ H ) ∗ P i ′ (8) P_{i}=(I-K*H)*P'_{i}\tag{8} Pi​=(I−K∗H)∗Pi′​(8)

在学习立即上述两组模型的时候一定要格外注意上下标，避免混淆！！！
其中x为目标状态真值，P为隐含状态的矩阵，F为运动模型函数（函数矩阵），u为控制增量（非控制模型可以省略），Q为系统噪声矩阵，x’为预测的下一时刻目标状态真值，P’为预测的隐含状态矩阵，z为实际测量值，H为目标状态真值到观测值的变换函数（函数矩阵），R为测量噪声矩阵，K为卡尔曼系数，y为观测值与预测值（经过变换）的差。I为单位矩阵。

方程理解

那么从公式3到公式8是一组完整的更新输出滤波结果的过程。滤波的过程分为两个部分——预测部分和更新部分。
那么为什么是这两部分呢？那么以行进位置为例。假设一个机器人行进了一段距离。机器人行走10s，以3m/s的速度匀速前进，通过运动公式可以求得机器人行进距离为30m。也就是我们通过运动方程预测的机器人行进30m。但是我们无论是时间还是速度这两个值的测量只是一个近似值，他们很有可能是有噪声的，加上机器人可能面对风吹，路面不平等问题。这个30m的预测值存在一个近似高斯分布的噪声。除了速度和时间之外，我们的还有个超声波测距传感器。他测得机器人行进了28.5m。那么这个28.5的观测值是从目标对象外部获得结果。但是由于超声波传感器本身精度是存在问题的。他的测量值会存在近似高斯分布的噪声。
那么从直觉上来看我们会觉得小车大概率会在28.5-30m中间的距离。

我们能够直接测到运动位置。

方程中P的引入

那么预测模型（3）借用运动模型（1）可以直接转化一下即可。那么更新模型（7）与观测模型（2）要如何联系起来呢。此处需要一个间接变量作为中间的桥梁。如果之前有接触过马尔科夫链或者马尔科夫模型，那么相信你应该能理解在当前结果是通过上一结果和上一状态推到而来。此处比较抽象，可以理解为显式的信息与隐式的信息结合可以推到出下一个显式的信息与隐式的信息。一般我们能够观测到的称之为显式。我们不能观测到，但是事物具有的属性，称之为隐式。例如我们无法测到炼钢炉内部的千度以上的高温，但是我们能够测量到炼钢炉出口处的相对较低的温度。通过显式的出口的温度变化来推测隐式的炉子内部的温度变化。
对应上述机器人来说，就是实际的运动状态位置X和运动隐含信息P。我们很难准确测到观测信息，但是可以通过对隐含信息（包含速度，加速度，能量，阻力等）这一抽象概念的推导辅助我们构建观测与预测的桥梁。观测到运动位置和推导的运动位置之间的存在的联系。也就是通过（4）（5）（6）（8），建立联系。这样无论是预测还是更新，我们都需要对X和P分别进行预测和更新。使得隐含信息也得到预测和更新，通过隐含信息可以推导优化显示信息。

方程中下标为何只有i与i-1

比较认真的小伙伴可能发现上述表述中提到马尔科夫时，夹带了一个关键信息。就是当前结果仅仅与前一个时刻结果有关。而不是前n个。众所周知，均值滤波中是要使用前n个信息滤波多半是比前两个滤波融合的效果要好的。那么EKF为什么不使用前n个呢？那就还要回到马尔科夫这一概念。EKF的使用场景多为符合马尔科夫条件的场景，还是以机器人运动为例，运动位置的结果是与上一时刻的运动隐含信息（包含速度，加速度，能量，阻力等）高度相关的，但是再往前的运动信息的是不具备推测意义。注意此处是有微观的概念的，与高中学习的匀速或者匀加匀减的理想状态不同，在理想状态下，是可以通过第前n个时刻的信息，结合运动方程，推测出当前结果的。也就是 y i = f ( x i − n ) y_{i}=f(x_{i-n}) yi​=f(xi−n​)。但是非理想状态下，能量每时每刻都在消耗，运动的环境也在不断改变。所以能提供有效信息的只有前一时刻。也就是 y i = f ( x i − 1 ) y_{i}=f(x_{i-1}) yi​=f(xi−1​)。并且时刻的划分越是微分得到近似结果越好。这也对应了实际工程中，高精度往往伴随了高采样频率。
与均值滤波相比，他的输出频率与输入频率是保持一致的。

噪声矩阵

在数学模型中噪声应当是个值或者向量。系统噪声的维度与目标状态真值的维度相同。观测噪声的维度与观测值的维度相同。之所以在滤波模型中变为矩阵是因为由于要计算两个高斯分布的和，所以这些噪声的影响是要应用于协方差P中的，所以将噪声向量转化为对角矩阵（diag），就可以实现噪声应用于协方差矩阵P中了。

关于y

y从公式上看是观测值与预测值的差，但是由于工程应用中，观测值很有可能和目标状态值的维度是不同的，或者物理意义是不同的。所以需要将目标状态值经过等价变换，使得他们可以相加减。也就是引入H矩阵（H变换函数）。此处可以举例温度图，例如温度是某一个具体值，但是颜色是RGB三个值组成的向量，那么从温度变化到颜色，肯定是有对应函数计算关系的。这个计算关系就是H矩阵。

卡尔曼系数

如上图所示两个高斯分布相加，即可得到滤波真值的高斯分布，滤波真值的高斯分布的中值代表了最有可能贴近真值的结果。那么我们如何将高斯分布相加表示成数学形势呢？
显然某个值或向量是不能表述一个值或者向量的分布，我们需要拓展维度才能表述，这也就呼应了协方差P，显然协方差P是用矩阵的形势来表达多个维度信息存在互相影响的信息。
所以卡尔曼系数通常是个矩阵。
那么有了卡尔曼系数，我们要如何得到高斯分布相加的结果呢。正常情况下求均值可以是计算A、B两个数的差C，然后给差C一个系数k，然后通过A+kC得到结果。这样就是同时包含了AB的信息。对应到EKF当中就是X’+Ky，也就是既包含预测的信息，也包含了观测的信息。

多个观测源

上面都是讲述一个运动方程，一个观测源的情况。拓展到多个观测源时，只需要配套的多个噪声矩阵R和多个变换矩阵H即可。每次有新的信息来到时，只需重复上述的预测与更新过程。只是对应R和H替换成相应的信号源即可。

实例讲解

单一观测源-目标值维度为1的值-观测值与目标值维度相同

数学模型：
X i = F ∗ X i − 1 + Q X_{i}=F*X_{i-1}+Q Xi​=F∗Xi−1​+Q
Z i = H ∗ X i + R Z_{i}=H*X_{i}+R Zi​=H∗Xi​+R
X为目标变量，Z为观测变量，F为系数，H为系数，Q为误差系数，R为误差系数
带入滤波模型中：预测方程和更新方程如下：
x i ′ = F ∗ x i − 1 x'_{i}=F*x_{i-1} xi′​=F∗xi−1​
P i ′ = F ∗ P i − 1 ∗ F T + Q P'_{i}=F*P_{i-1}*F^{T}+Q Pi′​=F∗Pi−1​∗FT+Q
更新模型：
y = z − H ∗ x i ′ y=z-H*x'_{i} y=z−H∗xi′​
K = P i ′ ∗ H T ∗ ( H ∗ P i ′ ∗ H T + R K=P'_{i}*H^{T}*(H*P'_{i}*H^{T}+R K=Pi′​∗HT∗(H∗Pi′​∗HT+R
x i = x i ′ + K ∗ y x_{i}=x'_{i}+K*y xi​=xi′​+K∗y
P i = ( I − K ∗ H ) ∗ P i ′ P_{i}=(I-K*H)*P'_{i} Pi​=(I−K∗H)∗Pi′​
应用场景解析：
此处可以对应测温模型的ekf滤波。x为实际温度状态，z为测量温度，P为温度的协方差（维度为1，也可以称为方差），Q和R分别为系统噪声和观测噪声，他们可以随着系统变化，也可以恒定不变。
对应代码原型如下：
kf.h文件

class KalmanFilter {
private:
  float X_;// state vector
  float P_;// state covariance matrix
  float F_;// state transistion matrix
  float Q_;// process covariance matrix
  float H_;// measurement matrix
  float R_;// measurement covariance matrix
public:
  KalmanFilter() {}//Constructor
  ~KalmanFilter() {}//Destructor
  void initParam();
  void Predict();
  void Update(const float z);
  float getX(){return X_;}
};

kf.cpp文件

void KalmanFilter::initParam(float X0)
{
    X_=X0;
    P_=1.7;//初始状态值需要手工估计
    H_=1.0;//对应转换公式
    F_=1.03;//对应运动方程
    Q_=0.003;//系统误差可以通过标定或手工试错
    R_=0.005;//噪声误差可以通过标定或手工试错
}
void KalmanFilter::Predict() {
  /**
  TODO:
    * predict the state
  */
  X_ = F_*X_;//对应公式（3）
  P_ = F_*P_*(F_) + Q_;//对应公式（4）

}
void KalmanFilter::Update(const float &z) {
  /**
  TODO:
    * update the state by using Kalman Filter equations
  */
  float z_pred = H_ * X_;//对应公式（5）
  float y = z - z_pred;//对应公式（5）
  float Ht = H_;//对应公式（6）
  float PHt =  P_ * Ht;//对应公式（6）
  float S = H_ * PHt + R_;//对应公式（6）
  float Si = 1/S;//对应公式（6）
  float K =  PHt * Si;//对应公式（6）
  X_ = X_ + (K * y);//对应公式（7）
  float I=1.0;
  P_ = (I- K * H_) * P_;//对应公式（8）
}

main.cpp

#include <iostream>
using namespace std;
int main()
{
    //制造数据
    float z[10]={4.0,4.09,4.2,4.33,4.44,4.57,4.67,4.74,4.83,4.99};
    //开始滤波
    KalmanFilter kf;
    kf.initParam(z[0]);//输入初始状态
    float result0=kf.getX();//获取状态值
    cout<<result0<<endl;//打印状态值到控制端
    for(int i=1;i<10;i++)
    {
        kf.Predict();
        kf.Update(z[i]);//输入观测状态并更新
        float result=kf.getX();//获取状态值
        cout<<result<<endl;//打印状态值到控制端
    }
}

单一观测源+目标状态值维度为n的向量+观测值维度为m的向量

由于实际应用中一个对象往往具备多个属性，更多的是多个维度描述以一个对象的情况，而且观测的时候也不一定是要观测这些属性，因此维度很有可能不同，所以编写一个通用的卡尔曼滤波模块会比较好。使用矩阵表示在所难免，Eigen库就是一个很好的选择。
将上述滤波模型转化为矩阵形式如下：
预测模型：
x i ′ = F ∗ x i − 1 x'_{i}=F*x_{i-1} xi′​=F∗xi−1​
P i ′ = F ∗ P i − 1 ∗ F T + Q P'_{i}=F*P_{i-1}*F^{T}+Q Pi′​=F∗Pi−1​∗FT+Q
更新模型：
y = z − H ∗ x i ′ y=z-H*x'_{i} y=z−H∗xi′​
K = P i ′ ∗ H T ∗ ( H ∗ P i ′ ∗ H T + R K=P'_{i}*H^{T}*(H*P'_{i}*H^{T}+R K=Pi′​∗HT∗(H∗Pi′​∗HT+R
x i = x i ′ + K ∗ y x_{i}=x'_{i}+K*y xi​=xi′​+K∗y
P i = ( I − K ∗ H ) ∗ P i ′ P_{i}=(I-K*H)*P'_{i} Pi​=(I−K∗H)∗Pi′​
应用场景解析：
注意此处的变量不再是一个值了，他们会变为向量和矩阵的形式，可以带入应用场景——二维平面的坐标位置。

可以注意到二维平面坐标通常会把X设置为x，y两个变量代表即可，为何要带上vx和vy（x方向上速度，y方向上速度）。是因为此处建立的关于坐标运动的数学模型是一个和速度有关的函数，即 x i = x i − 1 + f ( v x ) x_{i}=x_{i-1}+f(vx) xi​=xi−1​+f(vx)和 y i = x i − 1 + f ( v y ) y_{i}=x_{i-1}+f(vy) yi​=xi−1​+f(vy)，因此，如果你的坐标数学模型不包含速度，但是包含加速度也是可以相应替换，或者你是游戏引擎既不包含速度也不包含加速度，只是一个关于位置的函数，那么X也可以只有x，y两个变量。
除此之外，关于F中的 d e l t a t delta_{t} deltat​是因为F为表达关于X变化的数学模型，此处数学模型为匀速运动，即 x i = x i − 1 + v x ∗ d e l t a t x_{i}=x_{i-1}+vx*delta_{t} xi​=xi−1​+vx∗deltat​和 y i = y i − 1 + v y ∗ d e l t a t y_{i}=y_{i-1}+vy*delta_{t} yi​=yi−1​+vy∗deltat​之所以近似为匀速运动是因为对时间进行了微分，也就是 d e l t a t delta_{t} deltat​。对应为矩阵的表达形式就是上述的F。
我们还需要注意到Z是个二维向量，因为我们能够观测到具体位置，但是观测速度就比较困难，所以速度是存在于状态X中，但是不存在于观测Z中。那么X到Z存在转换关系，需要进行维度变化，那么H矩阵就不难构造了。

代码原型如下：
kf2.h

#include <vector>
#include <iostream>
#include <Eigen/Dense>

class kalman_filter {
public:

  Eigen::VectorXd X_;// state vector
  Eigen::MatrixXd P_;// state covariance matrix
  Eigen::MatrixXd F_;// state transistion matrix
  Eigen::MatrixXd Q_;// process covariance matrix
  Eigen::MatrixXd H_;// measurement matrix
  Eigen::MatrixXd R_;// measurement covariance matrix

  kalman_filter() {}//Constructor
  ~kalman_filter() {}//Destructor
  void initParam(const Eigen::VectorXd X0);
  void Predict();
  void Update(const Eigen::VectorXd &z);
  Eigen::VectorXd getX(){return X_;}
};

kf2.cpp

#include "kf2.h"
void kalman_filter::initParam(const Eigen::VectorXd X0)
{
    X_=X0;//设定维度&输入具体值
    P_ = Eigen::MatrixXd(4, 4);//设定维度
    P_ << 1, 0, 0,    0,
          0, 1, 0,    0,
          0, 0, 1000, 0,
          0, 0, 0, 1000;//输入具体值

    R_ = Eigen::MatrixXd(4,4);//设定维度
    H_ = Eigen::MatrixXd(2,4);//设定维度
    F_ = Eigen::MatrixXd(4,4);//设定维度
    Q_ = Eigen::MatrixXd(4,4);//设定维度
}

void kalman_filter::Predict() {
  /**
  TODO:
    * predict the state
  */
  X_ = F_*X_;
  P_ = F_*P_*F_.transpose() + Q_;

}

void kalman_filter::Update(const Eigen::VectorXd &z) {
  /**
  TODO:
    * update the state by using Kalman Filter equations
  */
  Eigen::VectorXd z_pred = H_ * X_;
  Eigen::VectorXd y = z - z_pred;
  Eigen::MatrixXd Ht = H_.transpose();
  Eigen::MatrixXd PHt =  P_ * Ht;
  Eigen::MatrixXd S = H_ * PHt + R_;
  Eigen::MatrixXd Si = S.inverse();
  Eigen::MatrixXd K =  PHt * Si;
  X_ = X_ + (K * y);
  long x_size = X_.size();
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(x_size, x_size);
  P_ = (I- K * H_) * P_;
}

main.cpp

#include "kf2.h"
int main()
{
    //制造观测数据
    std::vector<Eigen::Vector2d> inputSource;
    for(int i=0;i<10;i++)
    {
        Eigen::Vector2d Z; 
        Z(0)=2*i+3;
        Z(1)=3*i+7；
        inputSource.push_back(Z);
    }
    //开始滤波
    kalman_filter kf;
    kf.initParam(inputSource[0]);//输入初始状态
    kf.H_<< 1,0,0,0,
            0,1,0,0;
    kf.R = Eigen::Vector3d(0.0225,0.0225).asDiagonal();
    Eigen::Vector2d result0=kf.getX().head(2);//获取状态值,之所以对X取head（2）是因为X为4维，前两个元素为x，y位置
    cout<<result0<<endl;//打印状态值到控制端
    int deltaT=1;
    float noise_x=0.0005;
    float noise_y=0.0005;
    for(int i=1;i<10;i=i+deltaT)
    {
        kf.Predict();
        kf.F_<<1,0,deltaT,    0,
               0,1,     0,deltaT,
               0,0,     1,     0,
               0,0,     0,     1;
        Eigen::VectorXd G_=Eigen::VectorXd(4);
        G<<0.5*noise_x*deltaT*deltaT,0.5*noise_y*deltaT*deltaT,noise_x*deltaT,noise_y*deltaT;
        ekf_.Q_ =G*G.transpose();
        kf.Update(inputSource[i]);//输入观测状态并更新
        Eigen::Vector2d result=kf.getX().head(2);//获取状态值
        cout<<result<<endl;//打印状态值到控制端
    }
}

需要注意的是在初始化参数的时候，可以只有X和P是必须进行数值初始化的，其他参数可以是可以随着系统状态变化，或者是随着输入信号源变化的。

其他案例参见（二）

参考文章：
https://blog.csdn.net/m0_38087936/article/details/85479069
https://zhuanlan.zhihu.com/p/45238681
https://blog.csdn.net/AdamShan/article/details/78248421
https://blog.csdn.net/hanmoge/article/details/113242104

其他

最后补充一下，普通相机照相会携带exif信息，普通的图像处理是只处理图像内容，并不处理exif信息。导致相机内本身包含相机和图像的朝向Orientation变化关系，并不随图像处理改变。因此如果上传到网站或者markdown，可能处理好的图像又旋转了。
解决方案有两种：

1. 编辑图像本身的Orientation信息，1是原始位置，3是旋转180°，6是旋转90°(我可能有记错，可以自己尝试一下，或者查询exif Orientation)
2. 所幸png图像格式是不携带exif信息的，所以可以将图像另存为png格式即可。（png会导致图像信息变大，如果超过上传限制，可以再次转回jpg。相当于通过转换png，又转换jpg来丢掉exif信息）