卡尔曼滤波 python opencv 卡尔曼滤波 贝叶斯滤波

之前说到了贝叶斯滤波的原理和计算，最终我们发现，贝叶斯滤波在预测步和更新步，每一轮都需要进行多次无穷积分，这就要求我们清楚的知道每一步需要的概率密度函数，这样实在难以求解甚至无解析解，于是人们想了一些办法来解决。

为了求解贝叶斯滤波，人们的方法主要分成两种：

---

智力派：核心思想是做出假设，在一些特定情况下，是可以让贝叶斯滤波的计算大大简化的，回顾一下我们在之前贝叶斯滤波内容里的起点：

\[X_{k} = f(X_{k-1}) + Q_{K} →状态方程：描述随机过程 \\ Y_{k} = h(X_{k})+R_{k} →观测方程：描述观测结果 \]

如果我们的状态方程的f(X_k-1)，观测方程中的h(X_k)都是线性方程，Q_k和R_k依旧是正态分布的噪声，那么我们就可以把它改写成这种形式：

\[X_{k} = AX_{k-1} + Q_{K} →状态方程：描述随机过程 \\ Y_{k} = HX_{k}+R_{k} →观测方程：描述观测结果 \]

这种假设下，我们就得到了卡尔曼滤波（KF）。

但是这种假设太强了，很多情况下我们的f(X_k-1)，h(X_k)都是非线性的，但是依旧有办法，将其近似为线性计算，这样我们就得到了非线性的卡尔曼滤波：扩展卡尔曼滤波（EKF）和无迹卡尔曼滤波（UKF）。

---

莽夫派：核心思想是硬算，直接对无穷积分作数值积分。比如高斯积分，蒙特卡洛积分（粒子滤波），直方图滤波（将密度函数转为直方图进行积分）。

这里我们要说的是卡尔曼滤波，就先不管莽夫派的算法啦。

1.线性的卡尔曼滤波

1.1.思路及原理

回顾一下贝叶斯滤波的思想，我们为了解决随机过程中的状态估计问题，首先需要两个方程：

\[X_{k} = f(X_{k-1}) + Q_{K} →状态方程：描述随机过程 \\ Y_{k} = h(X_{k})+R_{k} →观测方程：描述观测结果 \]

然后我们通过两步来进行状态估计，首先是预测步，我们通过初值，或前一时刻的后验，来求出这一时刻的先验：

\[f_{k}^{-}(x) = \int_{-\infty }^{\infty } f_{Q}[x-f(v)]f_{X_{k-1}}^{+}(v) dv \]

之后是更新步，我们通过这一时刻的先验，以及观测方程，计算出这一时刻的后验，来作为此时的估计结果：

\[f_{k}^{+}(x) = n\cdot f_{R}[y_{k} - h(x)]f_{k}^{-}(x) \\n =( \int_{-\infty}^{+\infty} f_{R}[y_{k} - h(x)]f_{k}^{-}(x) dx)^{-1} \]

在卡尔曼滤波中，我们假设：

• f(X_k-1)，h(X_k)是线性函数，即 f(X_k-1) = F·X_k-1，h(X_k) = H·X_k
• Q_k和R_k服从正态分布，Q_k ~ N(0, Q)，R_k ~ N(0, R)

我们假设X_k-1 ~ N(μ_k-1⁺, σ_k-1⁺)那么预测步：

\[f_{k}^{-}(x) = \int_{-\infty }^{\infty } f_{Q}[x-f(v)]f_{X_{k-1}}^{+}(v) dv \\=\int_{-\infty }^{\infty } \frac{1}{\sqrt{2\pi Q}}e^{-\frac{(x-F\cdot v)^{2}}{2Q}}\cdot \frac{1}{\sqrt{2\pi \sigma _{k-1}^{+}}}e^{-\frac{(v-\mu_{k-1}^{+})^{2}}{2\sigma _{k-1}^{+}}} dv \]

看起来有些复杂，实际上它还是一个正态分布，且服从：

\[X_{k}^{-} \sim N(F\mu_{k-1}^{+}, F^{2}\sigma _{k-1}^{+}+Q) \]

这就是先验的概率分布情况了，我们记它为N(μ_k^-, σ_k^-)，那么更新步：

\[f_{k}^{+}(x) = n\cdot f_{R}[y_{k} - h(x)]f_{k}^{-}(x) \\=n \cdot \frac{1}{\sqrt{2\pi R}}e^{-\frac{(y_{k}-H\cdot x)^{2}}{2R}}\cdot \frac{1}{\sqrt{2\pi \sigma _{k}^{-}}}e^{-\frac{(v-\mu_{k}^{-})^{2}}{2\sigma _{k}^{-}}} \\n = (\int_{-\infty}^{+\infty} \frac{1}{\sqrt{2\pi R}}e^{-\frac{(y_{k}-H\cdot x)^{2}}{2R}}\cdot \frac{1}{\sqrt{2\pi \sigma _{k}^{-}}}e^{-\frac{(v-\mu_{k}^{-})^{2}}{2\sigma _{k}^{-}}}dx)^-1 \]

看起来还是很复杂的一坨，实际上它依旧是一个正态分布，且服从：

\[X_{k}^{+} \sim N(\frac{H \sigma _{k}^{-} y_{k}+R\mu_{k}^{-}}{H^{2} \sigma _{k}^{-}+R}, \frac{R\sigma_{k}^{-}}{H^{2} \sigma _{k}^{-}+R}) \]

我们将它记为N(μ_k⁺, σ_k⁺)，整理一下：

\[\mu_{k}^{+} = \frac{H\sigma_{k}^{-}}{H^{2}\sigma_{k}^{-}+R}(y_{k}-H\mu_{k}^{-})+\mu_{k}^{-} \\ \sigma_{k}^{+} = (1- \frac{H^{2}\sigma_{k}^{-}}{H^{2}\sigma_{k}^{-}+R})\sigma_{k}^{-} \]

我们发现他们都有相同的部分，于是我们定义一个K：

\[\mu_{k}^{+} = K(y_{k}-H\mu_{k}^{-})+\mu_{k}^{-} \\ \sigma_{k}^{+} = (1- KH)\sigma_{k}^{-} \\ K = \frac{H\sigma_{k}^{-}}{H^{2}\sigma_{k}^{-}+R} \]

这就是卡尔曼增益。这里我们可能发现了一些卡尔曼滤波之所以比贝叶斯滤波简化了运算的原因：

• 它假设状态方程和观测方程都是线性的
• 状态、观测、噪声均服从正态分布，而相互独立的正态分布有很好的封闭性，这样我们在计算中，只需要保存好他们的期望与方差，进行简单运算就可以了

最后我们再整理一下卡尔曼滤波的过程：

预测步：

\[状态方程：\\ X_{k} = F\cdot X_{k-1} + Q_{k} ，Q_{k} \sim N(0,Q) \\ 预测结果（先验）： \\X_{k}^{-} \sim N(F\mu_{k-1}^{+}, F^{2}\sigma _{k-1}^{+}+Q) \]

更新步：

\[观测方程：\\ Y_{k} = H\cdot X_{k}+R_{k}，R_{k} \sim N(0,R)\\ 后验：\\ X_{k}^{+} \sim N(K(y_{k}-H\mu_{k}^{-})+\mu_{k}^{-},(1- K)\sigma_{k}^{-}) \]

这里我们把卡尔曼增益K写了两种写法，我们看一下第二种写法：

• 当R>>σ_k^-时，K趋近于0，这时我们的期望：
  \[\mu_{k}^{+} = \mu_{k}^{-} \]
  意味着我们更倾向于相信预测。
• 当R<<σ_k^-时，K趋近于 1/H，这时我们的期望：
  \[\mu_{k}^{+} = \frac{y_{k}}{H} \]
  意味着我们更倾向于相信观测。

可以看到它也很好地保留了贝叶斯滤波的“滤波”特性。

但是我们在实际使用中，最长使用的是矩阵形式的卡尔曼滤波，接下来由上面的公式改写成矩阵形式吧~

1.2 矩阵形式的卡尔曼滤波

在矩阵形式的计算中，我们的期望将不再是一个数字标量，而是一个向量。方差也将变成协方差矩阵，状态方程与观测方程中的F和H，也将变成矩阵F和H，我们加粗表示。

\[\mu _{k} → \vec{\mu _{k}} \\ \sigma _{k} → \Sigma _{k} \]

那么预测步：

\[期望：\vec{\mu_{k}^{-}} = F\cdot \vec{\mu_{k-1}^{+}}\\ 方差：\Sigma _{k}^{-} = F\Sigma _{k-1}^{+}F^{T} + Q\\ 即：X_{k}^{-} \sim N(F\cdot \vec{\mu_{k-1}^{+}}, F\Sigma _{k-1}^{+}F^{T} + Q) \]

更新步：

\[期望：\vec{\mu_{k}^{+}} = \vec{\mu_{k}^{-}} + K(\vec{y_{k}}-H\vec{\mu_{k}^{-}})\\ 方差：\Sigma _{k}^{+} = (I-KH)\Sigma _{k}^{-}\\ K =\Sigma _{k}^{-}H^{T}(H\Sigma _{k}^{-}H^{T}+R)^{-1} \\ 即：X_{k}^{+} \sim N(\vec{\mu_{k}^{-}} + K(\vec{y_{k}}-H\vec{\mu_{k}^{-}}), (I-KH)\Sigma _{k}^{-}) \]

接下来我们写一个小demo，来演示一下，对于一个含有正态噪声的型号，卡尔曼滤波如何处理。

1.3 代码实现（对包含正态噪声的信号进行卡尔曼滤波）

我们来手撸一个简单的卡尔曼滤波器，这次滤波器的Demo里，我们的真实值取：

\[x = t^{2} \]

我们假设它是一个随时间二次增长的值，因为是模拟，观测部分我们直接在真实值上添加一个正态噪声，来进行模拟，由于我们的观测值就是类似温度计直接测量温度那样，所以我们的观测方程：

\[Y_{k} = HX_{k}+R_{k},H=1 \]

我们的状态方程，我们先粗糙的建立一个模型（虽然上面我们为了模拟，给出了真实值，但实际上我们是不知道状态是如何变化的）：

\[X_{k} = FX_{k-1} + Q_{K},F=1 \]

思考一下我们需要计算的内容：

---

预测步：

\[\\X_{k}^{-} \sim N(F\mu_{k-1}^{+}, F^{2}\sigma _{k-1}^{+}+Q) \]

我们需要计算这两项内容，其中F我们在上面已经设置为1，第一次的先验分布我们自己设置初值，后续均由递推获得：

后验的均值：X_k_minus = F  *  X_pre_plus  
     方差：P_k_minus = F*F  *  P_pre_plus  +  Q

---

更新步：

\[X_{k}^{+} \sim N(K(y_{k}-H\mu_{k}^{-})+\mu_{k}^{-},(1- K)\sigma_{k}^{-})\\ K = \frac{H\sigma_{k}^{-}}{H^{2}\sigma_{k}^{-}+R} \]

我们需要计算这三项内容：

卡尔曼增益：K = P_k_minus * H / (H*H * P_k_minus + R)
先验的均值：X_k_plus = X_k_minus + K*(Y_k - H * X_k_minus)
     方差： P_k_plus = (1 - K*H) * P_k_minus */

其中H我们也已经设置为1，状态方程与观测方程的噪声Q，R我们在程序中设置，后验由更新步计算得出。

接下来是完整代码（C++）：

#include <iostream>
#include <random>
#include <vector>
using namespace std;
void kalmanFilter(const vector<float>& srcX,const vector<float>& Y, vector<float>& plusX){
    /* 观测方程Y(X_k) = H(X_k) + R：
     * 由于提供模拟观测数据，这里直接使用模拟数据, 由于模拟观测是直接由真值加噪声获得，取H = 1，R是均值为0，方差为1的正态噪声
     * 状态方程：X_k = F * X_pre + Q
     * 我们设状态方程为线性方程，且F = 1， Q是均值为0，方差为1的正态噪声 */ 
    float F = 1.0, Q = 1.0, H = 1.0, R= 1.0;
   
    // 初始值，假设X0~N(0.01, 0.01^2)
    plusX[0] = 0.01;
    float plusP = 0.0001;
    
    /* 后验的均值：X_k_minus = F  *  X_pre_plus  
     *     方差：P_k_minus = F*F  *  P_pre_plus + Q
     * 卡尔曼增益：K = P_k_minus * H / (H*H * P_k_minus + R)
     * 先验的均值：X_k_plus = X_k_minus + K*(Y_k - H * X_k_minus)
     *     方差： P_k_plus = (1 - K*H) * P_k_minus */
    float minusX, minusP, K; // plusX在数组更新，plusP已经在初始值处定义
    for(size_t i = 1; i < srcX.size(); i++){    
        minusX = F * plusX[i-1];
        minusP = F*F * plusP + Q;
        K = minusP * H / (H*H * minusP + R);
        plusX[i] = minusX + K * (Y[i] - H * minusX);
        plusP = (1 - K * H) * minusP;
    }
}
int main() {
    /* 真实数据X = n^2，保存到srcX
    * 在真实数据上添加正态噪声，保存到Y，作为模拟观测
    * 共 50 个数据 */
    vector<float> srcX;
    vector<float> Y;
    float n = 0.02;
    default_random_engine gen;
    //观测噪声的均值与方差
    normal_distribution<float> dis(0,0.1);
    while(n <= 1.0){       
        srcX.push_back(n*n);
        Y.push_back(n*n + dis(gen)); 
        n += 0.02;
    }
    // 保存滤波结果的数组
    vector<float> plusX(srcX.size(), 0);
    kalmanFilter(srcX, Y, plusX);
    /*
     * Do something with ur srcX, Y ,pulsX. 
     */
    return 0;
}

在上面的程序里，我们的观测方程与状态方程的噪声均设置为N（0,1），我们来看一下结果：

呃…… 似乎效果不怎么好，因为我们对于状态方程的建模太过粗糙了，并且我们的状态方程的噪声设置与观测方程一样，我们试一试将状态方程的噪声Q设置为N(0,0.01)，再来试一下：

似乎平滑了很多，但是在曲线结尾，偏差越来越大。这是由于我们给Q设置了较小的方差，导致滤波器更偏向于相信预测值，然而真实值是一个平方函数，这就不是我们目前，粗糙建模的状态方程能预测的准的了。

而相对的，如果我们把R设置为N(0,0.01)，那么：

滤波器将非常倾向于观测值，滤波后的结果几乎与观测值一致了。

之前在贝叶斯滤波那里就曾经说过，初值的选择对滤波结果的影响不大，假设我们的初值定为5,那么：

可以看出，实际上在三四轮计算后，我们的滤波结果又趋近于真实值了，可能这就是贝叶斯滤波的强大之处了吧。

不过在开头处，我们说过，很多情况下，状态方程与观测方程其实是非线性的，也就无法简单的使用卡尔曼滤波，那么，就需要扩展卡尔曼滤波的登场啦！

2. 扩展卡尔曼滤波

2.1 思想

在基础的卡尔曼滤波里，我们做出了一个很强的假设：状态方程和观测方程都是线性的。

然而实际上很多情况是不符合线性变化这个规律的，比如SLAM的位姿等等。

在不符合线性规律的变化下，我们得到的状态也将不符合正态分布，这样我们就没办法预测——更新——再预测的递推下去了。

于是人们又想出这样的方法：我们可不可以在某种特殊情况，或者某个点处，将其近似为线性计算呢？

比如我们想要拟合一条非线性的曲线，对于某个点处，我们可不可以用曲线的切线来近似的表示这一点处的一些性质呢？

这就是扩展卡尔曼滤波的想法了。

2.2原理

既然是非线性过程，那么我们的核心，状态方程和观测方程就又变回了贝叶斯滤波里的原始形式：

\[X_{k} = f(X_{k-1}) + Q_{K} →状态方程：描述随机过程 \\ Y_{k} = h(X_{k})+R_{k} →观测方程：描述观测结果 \]

扩展卡尔曼滤波的想法，就是如何将这两个非线性函数，近似成线性函数，也就是线性化。

我们先假设某一时刻的，前一时刻的状态X_k-1服从这样的正态分布，：

\[X_{k-1} \sim N(\hat{x}^{+}_{k-1}, p^{+}_{k-1}) \]

我们对状态方程在X_k-1处进行泰勒展开：

\[f(X_{k-1}) \approx + f(\hat{x}^{+}_{k-1}) + f'(\hat{x}^{+}_{k-1})\cdot (X_{k-1}-\hat{x}^{+}_{k-1}) \\\approx f'(\hat{x}^{+}_{k-1})\cdot X_{k-1} - f'(\hat{x}^{+}_{k-1})\cdot\hat{x}^{+}_{k-1} + f(\hat{x}^{+}_{k-1}) \]

这样我们就得到了这样形式的方程：

\[f(X_{k-1}) \approx A\cdot X_{k-1} + B\\ A = f'(\hat{x}^{+}_{k-1}) \\B = f'(\hat{x}^{+}_{k-1})\cdot\hat{x}^{+}_{k-1} + f(\hat{x}^{+}_{k-1}) \]

注意其中的A和B都是常量，也就是说我们通过泰勒展开，用线性函数近似了原函数。

这样我们的状态方程就变成了：

\[X_{k} = A\cdot X_{k-1} + B + Q_{k} \]

于是我们又可以像上面线性的卡尔曼滤波一样进行预测步的计算：

\[X_{k} \sim N(A\hat {x}^{+}_{k-1}+B, A^{2}p^{+}_{k-1}+Q) \]

它的均值：

\[A\hat {x}^{+}_{k-1}+B = f'(\hat{x}^{+}_{k-1})\cdot \hat{x}^{+}_{k-1} - f'(\hat{x}^{+}_{k-1})\cdot\hat{x}^{+}_{k-1} + f(\hat{x}^{+}_{k-1})\\ = f(\hat{x}^{+}_{k-1}) \]

可以发现我们并不需要计算B，那么对于预测步：

\[均值：\hat{x}^{-}_{k} = f(\hat{x}^{+}_{k-1})\\方差：p^{-}_{k} = A^{2}p^{+}_{k-1} + Q \\其中：A = f'(\hat{x}^{+}_{k-1}) \]

接下来是观测方程，我们同样对其进行泰勒展开，来用展开式近似原式：

\[Y_{k} = h(X_{k}) + R_{k} \\h(X_{k}) \approx h(\hat{x}^{-}_{k}) + h'(\hat{x}^{-}_{k})(X_{k} - - \hat{x}^{-}_{k})\\\approx h'(\hat{x}^{-}_{k}) X_{k} - h'(\hat{x}^{-}_{k})\hat{x}^{-}_{k} + h(\hat{x}^{-}_{k})\\=C\cdot X_{k} + D\\Y_{k} = C\cdot X_{k} + D + R_{k} \]

于是更新步：

\[均值：\hat{x}^{+}_{k} = \hat{x}^{-}_{k} + K(y_{k} - C\hat{x}^{-}_{k})\\方差：p^{+}_{k} = (1-KC)p^{-}_{k}\\卡尔曼增益：K = \frac {Cp^{-}_{k}}{C^{2}p^{-}_{k} + R}\\其中： C = h'(\hat{x}^{-}_{k}) \]

这样我们把非线性的方程通过近似，又转化回了线性的情况，参考一下线性的卡尔曼滤波，可以看出我们只需要多计算两个导数部分A和C就可以了。

---

对于矩阵形式的扩展卡尔曼滤波则：

预测步：

\[均值：\vec{\hat{x}^{-}_{k}} = f(\vec{\hat{x}^{+}_{k-1}})\\方差：\Sigma^{-}_{k} = A\Sigma^{+}_{k-1}A^{T} + Q \\其中：A为对 \hat{x}^{+}_{k-1}求导所得雅克比矩阵：\\A = {\begin{pmatrix}\frac{\partial f_{1}}{\partial \hat{x}_{k-1,1}}& \frac{\partial f_{1}}{\partial \hat{x}_{k-1,2}} & \dots & \frac{\partial f_{1}}{\partial \hat{x}_{k-1,n}}\\ \frac{\partial f_{2}}{\partial \hat{x}_{k-1,1}} & \frac{\partial f_{2}}{\partial \hat{x}_{k-1,2}} & \dots & \frac{\partial f_{2}}{\partial \hat{x}_{k-1,n}}\\ \vdots & \vdots & & \vdots \\ \frac{\partial f_{n}}{\partial \hat{x}_{k-1,1}} & \frac{\partial f_{n}}{\partial \hat{x}_{k-1,2}} & \dots & \frac{\partial f_{n}}{\partial \hat{x}_{k-1,n}}\end{pmatrix}} \]

更新步：

\[均值：\vec{\hat{x}^{+}_{k}} = \vec{\hat{x}^{-}_{k}} + K(\vec{y_{k}} - C\vec{\hat{x}^{-}_{k}})\\方差：\Sigma^{+}_{k} = (I-KC)\Sigma^{-}_{k}\\卡尔曼增益：K = C\Sigma^{-}_{k}(C\Sigma^{-}_{k}C^{T} + R)^{-1}\\其中： C为对\hat{x}^{-}_{k}求导的雅克比矩阵(不一定为方阵)：\\C = \begin{pmatrix}\frac{\partial h_{1}}{\partial \hat{x}_{k,1}}& \frac{\partial h_{1}}{\partial \hat{x}_{k,2}} & \dots & \frac{\partial h_{1}}{\partial \hat{x}_{k,n}}\\ \vdots & \vdots & & \vdots \\ \frac{\partial h_{m}}{\partial \hat{x}_{k,1}} & \frac{\partial h_{m}}{\partial \hat{x}_{k,2}} & \dots & \frac{\partial h_{m}}{\partial \hat{x}_{k,n}}\end{pmatrix} \]

2.3 总结

与无偏最优估计的卡尔曼滤波不同，扩展卡尔曼滤波的结果只是通过泰勒展开的近似值，它注定无法像卡尔曼滤波一样效果理想，不过它也继承了卡尔曼滤波非常易于计算的特点。

回顾一下滤波算法能够work的点：

• 通过预测得到的先验与观测的结果，得到一个比两个结果都可信的后验
• 通过加权来控制两部分的可信度（实际上就是通过卡尔曼增益来调节的）

这样的方法其实并不只用于状态估计，比如我们最开始在贝叶斯滤波部分讨论的温度计测量温度的问题，虽然我们不能逆转时间在同一时间测10次温度，我们可不可以准备10个温度计来一下就测量10次呢？我们应该怎么评价每个温度计是可信还是不可信，最终我们应该如何加权平均来得到测量温度呢？

这其实就是滤波算法的另一个用处：数据融合(Data Fusion)。

在SLAM的状态估计问题中，我们更需要一个能利用全部历史信息，来得到更精确结果的估计方法，而滤波方法有其局限性，其计算只用到了上一步的信息，并没有利用之前更久的信息，感觉缺乏“大局观”。

于是在SLAM的状态估计中，目前更偏向于使用另一种思路：非线性优化。

通过将计算值（预测）与实际值（观测）相比较，构建一个最小二乘问题，然后通过所有的历史信息，来优化我们的位姿，岂不美哉？

不过这就是另外一个系列的内容啦。