【机器学习】卡尔曼滤波对运动轨迹优化（Python）

前言

在上一篇文章中，我们已经对卡尔曼滤波的Filter问题进行了数学推导求解【机器学习】卡尔曼滤波原理推导-优快云博客，得到了卡尔曼黄金五式。接下来，我们就案例而言，来看看卡尔曼滤波如何应用到生活当中。

案例引入

小佳从拼夕夕砍来了一辆小汽车，这车除了能开啥子功能都没有。每次小佳都是以一定的速度去开车，并且无时无刻都在想着当前的位置信息是多少。作为一个大聪明文科生，小佳根本不知道运动学公式。于是他买了一个GPS定位器，实时返回位置信息。但没想到这个拼夕夕9.9包邮的定位器不准。人都被创飞好几米远了还说离前方人员还有几百米。为了解决这个问题，小佳学习物理，打算自己计算位置信息。可是，他家住在山旮旯附近，路面凹凸不平；狭管效应严重，天天七级台风能把人吹成大聪明，车辆的运动状态根本就不满足严格的运动学公式。智商堪比爱因斯坦的小佳决定综合GPS，利用卡尔曼滤波，对小车的运动轨迹实现较为精准的预测。

不难看到，预测的难度实际上就是在于，即便小佳每次都以一定的油门行驶，但是路面的凹凸不平，和能把人吹成大聪明的7级太分会对小车的实际速度受到随机扰动，所以即便是我们知道在$t-1$时刻的位置信息，也很难准确估计出$t$时刻的位置信息。而对于9.9包邮的GPS又存在一定的误差。

小佳要计算的位置信息包括当前的所在的位置和当前的速度是多少。我们假设小佳做的是匀速直线运动，那么依据运动学公式$s=s_0+v_{0}t$（假设每1秒计算一次，那么t=1，下文就不不是出来了），可以得到下一个时刻位置
$$s_t=s_{t-1}+v_{t-1}$$
我们用t表示当前时刻，t-1表示上一个时刻，也就是说当前的位置$s_t$就等于上一个时刻的位置$s_{t-1}$，加上上一个时刻的速度$v_t=v_{t-1}$。

而对于速度信息，很显然因为是匀速直线运动，所以直接就等于$v_{t-1}$。我们前面说过，这只是一个理想状态下的情况，我们是存在一定的随机扰动项的。也就是说，最终实际上我们可以写成
$$\begin{gathered} s_t=s_{t-1}+v_{t-1}+u_1; \\ {v}_t=v_{t-1}+u_2;\text{\hspace{1em}(1)} \end{gathered}$$
其中$u_1\sim N(0,Q_1),u_2\sim N(0,Q_2)$，也就是在自然界中，我们认为这些随机误差是服从正态分布的。所以每一项都加上一个正态分布。

同样的，对于GPS而言，在没有误差的情况下，设GPS定位出来的信息位置用$x$表示，速度用$y$表示。理论上t时刻的位置信息就直接等于
$$\begin{gathered} x_t=s_t \\ {y}_t=v_t \end{gathered}$$
但我们说过，GPS是存在误差的，所以
$$\begin{gathered} x_t=s_t+{\omega }_1 \\ {y}_t=v_t+{\omega }_2\text{\hspace{1em}(2)} \end{gathered}$$
其中${\omega }_1\sim N(0,R_1),{\omega }_2\sim N(0,R_2)$。我们依然认为误差服从正态分布。

那么，依据卡尔曼滤波，我们需要将其转化成矩阵相乘的形式。对于公式1
$$\left(\begin{array}{c}{s}_t\\ v_t\end{array}\right)=\left(\begin{array}{cc}1& 1\\ 0& 1\end{array}\right)\left(\begin{array}{c}{s}_{t-1}\\ v_{t-1}\end{array}\right)+\left(\begin{array}{c}{u}_1\\ u_2\end{array}\right)$$
为了与我们前面推导的符号一致，我们用其他符号代换，在上面这个公式中，第一个括号用$z_t$代替，第二个括号用$A$代替，第三个括号用$z_{t-1}$代替（要与z时刻一致符号），第四个括号直接用$u$代替，所以得到
$$z_t=A{z}_{t-1}+u$$
其中$u\sim N(0,Q)$

对于公式2我们同理
$$\left(\begin{array}{c}{x}_t\\ y_t\end{array}\right)=\left(\begin{array}{cc}1& 0\\ 0& 1\end{array}\right)\left(\begin{array}{c}{s}_t\\ v_t\end{array}\right)+\left(\begin{array}{c}{\omega }_1\\ {\omega }_2\end{array}\right)$$
用其他符号代换，第一个括号用$x_t$代替，第二个括号用$C$代替，第三个括号用$z_t$代替（要与前面一致符号），第四个用$v$代替，得
$$x_t=C{z}_t+v$$
其中$v\sim N(0,R)$

所以我们就得到线性关系式
$$\begin{gathered} z_t=A{z}_{t-1}+u \\ {x}_t=C{z}_t+v\text{\hspace{1em}(3)} \end{gathered}$$
绘制成卡尔曼滤波的模型图，如图所示

即在第一个时刻，我们能从z1的值得到x1和z2。然后不断地迭代下去，最终得到zt。我们的目的就是计算出前面t个时刻的小佳汽车的实际位置信息。

我们不妨想想，如何通过GPS和运动学公式去获取更加精准的位置？从频率派的角度而言。很显然，我们可以用加权平均的思想，也就是实际的位置信息P
$$P=K\ast x_t+(1-K)\ast z_t$$
K是一个系数，里面的$K\in [0,1]$，如果我们认为GPS里面的误差更小，更加可信，那么K的值就大一些。那如果我们认为运动学的误差更小，更可信，自然令K小一些。我们再将其转化一些
$$P=K\ast x_t+z_t-K{z}_t=z_t+K(x_t-z_t)$$
实际上，这里的K，被称为卡尔曼增益系数。

问题解决

如果看这一段看得云里雾里，看下一段的案例流程也许就懂了

本篇文章将从贝叶斯派的角度出发去解决这个问题，因为我们前面的推导就是从贝叶斯派的角度去推导。

对于贝叶斯派而言，上面的问题实际上就是卡尔曼滤波的Filter问题。也就是求出$P(z_t|x_1,\cdots ,x_t)$。

就是在给定了公式3的关系式和GPS前t个时刻的观测值$x$，去求出$z_t$。如果你看过上一篇的推导，你会发现$P(z_t|x_1,\cdots ,x_t)$是可以迭代求解的，比如只要我们求出$P(z_{t-1}|x_1,\cdots ,x_{t-1})$，就可以求出$P(z_t|x_1,\cdots ,x_t)$。所以最终不断递推到$P(z_1|x_1)$。

所以我们的只需要求出$P(z_1|x_1)$，就可以求出$P(z_2|x_1,x_2),......,P(z_t|x_1,\cdots ,x_t)$。保留我们每一个时刻的值，就是我们最终的小车运动的位置信息轨迹。

那么如何求出$P(z_1|x_1)$，如果你看过上一篇的推导，你会发现要求出$P(z_1|x_1)$，就要先求出$P(z_1|x_0)$；要求出$P(z_2|x_1,x_2)$，就要求出$P(z_t|x_1)$；实际上，要求出$P(z_t|x_1,\cdots ,x_t)$，就要求出优先求出$P(z_t|x_1,\cdots ,x_{t-1})$。

它们都是符合正态分布的。对于正态分布，我们只需要知道它们的期望和协方差矩阵即可，因为期望是概率最大的位置，我们可以将其当作就是卡尔曼滤波之后的值。对于$P(z_t|x_1,\cdots ,x_{t-1})$，我们就设定它的期望和协方差为${\bar{\mu }}_t，{\bar{\Sigma }}_t$。对于$P(z_t|x_1,\cdots ,x_t)$，我们设定它的期望和协方差矩阵为${\hat{\mu }}_t，{\hat{\Sigma }}_t$

那么按照流程，我们先求解$P(z_t|x_1,\cdots ,x_{t-1})$，在上一篇文章我们推导出来过，在这里直接给出它的参数公式
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_t=& A{\hat{\mu }}_{t-1}\\ {\bar{\Sigma }}_t=& A{\hat{\Sigma }}_{t-1}{A}^T+Q\end{array}\end{array}$$
得到$P(z_t|x_1,\cdots ,x_{t-1})$，再计算$P(z_t|x_1,\cdots ,x_t)$
$$\begin{gathered} K={\bar{\Sigma }}_t{C}^T{\left(C{\bar{\Sigma }}_t{C}^T+R\right)}^{-1} \\ {\hat{\mu }}_t={\bar{\mu }}_t+K(x_t-C{\bar{\mu }}_t) \\ {\hat{\Sigma }}_t=(I-KC){\bar{\Sigma }}_t \end{gathered}$$
我们前面说过，我们是要递归求解的，也就是不断递归到第一个时刻$P(z_1|x_1)$，我们来看看对于第一个时刻$P(z_1|x_0)$的参数求解
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_1=& A{\hat{\mu }}_0\\ {\bar{\Sigma }}_1=& A{\hat{\Sigma }}_0{A}^T+Q\end{array}\end{array}$$
这里的${\hat{\mu }}_0$和${\hat{\Sigma }}_0$是0时刻的值，我们是没有的，因此，我们需要给定${\bar{\mu }}_1$和${\bar{\Sigma }}_1$的值

案例流程

流程：

​ ①预测，计算$P(z_t|x_1,\cdots ,x_{t-1})$对应的参数
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_t=& A{\hat{\mu }}_{t-1}\\ {\bar{\Sigma }}_t=& A{\hat{\Sigma }}_{t-1}{A}^T+Q\end{array}\end{array}$$
​ ②更新，计算$P(z_t|x_1,\cdots ,x_t)$对应的参数
$$\begin{gathered} K={\bar{\Sigma }}_t{C}^T{\left(C{\bar{\Sigma }}_t{C}^T+R\right)}^{-1} \\ {\hat{\mu }}_t={\bar{\mu }}_t+K(x_t-C{\bar{\mu }}_t) \\ {\hat{\Sigma }}_t=(I-KC){\bar{\Sigma }}_t \end{gathered}$$
不断迭代①、②，直到算到t个时刻。

下面我们来举个例子

时刻1：

假定我们在时刻1，我们需要计算$P(z_1|x_0)$，因为$x_0$是不存在的，实际上就是计算$P(z_1)$，这个初始的值，需要我们自己给（这一步被称为预测）

假设
$${\bar{\mu }}_1=1，{\bar{\Sigma }}_1=2\text{\hspace{1em}(4)}$$
得到了期望和方差。接下来计算$P(z_1|x_1)$，来看公式
$$\begin{gathered} K={\bar{\Sigma }}_1{C}^T{\left(C{\bar{\Sigma }}_1{C}^T+R\right)}^{-1} \\ {\hat{\mu }}_1={\bar{\mu }}_1+K(x_1-C{\bar{\mu }}_1) \\ {\hat{\Sigma }}_1=(I-KC){\bar{\Sigma }}_1 \end{gathered}$$
我们将公式4得到的值代进里面（这一步被称为更新）
$$\begin{gathered} K=2\ast C^T{\left(C\ast 2\ast C^T+R\right)}^{-1}=3 \\ {\hat{\mu }}_1=1.5+3\ast (x_1-C\ast 1.5)=4 \\ {\hat{\Sigma }}_1=(I-3\ast C)\ast 2=4\text{\hspace{1em}(5)} \end{gathered}$$
好现在得到了$P(z_1|x_1)$，里面对应期望和协方差矩阵就是我们需要的东西。里面的期望我们认为是概率最大的地方，所以我们认为这个期望就是我们优化后的值

时刻2：

现在计算第2个时刻.先计算$P(z_2|x_1)$
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_2=& A{\hat{\mu }}_1\\ {\bar{\Sigma }}_2=& A{\hat{\Sigma }}_1{A}^T+Q\end{array}\end{array}$$
里面的${\hat{\mu }}_1，{\hat{\Sigma }}_1$就是我们公式5中计算出来的，将其带进去，得到
$$\begin{array}{c}\begin{array}{rl}{\bar{\mu }}_2=& A{\hat{\mu }}_1=A\ast 4=10\\ {\bar{\Sigma }}_2=& A{\hat{\Sigma }}_1{A}^T+Q=A\ast 4\ast A^T+Q=9\end{array}\end{array}\text{\hspace{1em}(6)}$$
再计算$P(z_t|x_1,x_2)$
$$\begin{gathered} K={\bar{\Sigma }}_2{C}^T{\left(C{\bar{\Sigma }}_2{C}^T+R\right)}^{-1} \\ {\hat{\mu }}_2={\bar{\mu }}_2+K(x_2-C{\bar{\mu }}_2) \\ {\hat{\Sigma }}_2=(I-KC){\bar{\Sigma }}_2 \end{gathered}$$
我们将公式6中的值带进去即可得到对应的期望和协方差矩阵。

然后以此反复，得出t个时刻的所有期望，就是我们综合GPS和运动学公式得到的位置信息。

代码实现

结果如图，可以看到，滤波之后的值相对平滑，并且每一个时刻的值都是更加靠近直线匀速运动的，因为我们在代码中设定的匀速运动的方差更小，而GPS的可信度相对差一些。

import numpy as np
import matplotlib.pyplot as plt
from scipy import stats
plt.rcParams['font.sans-serif'] = ['SimHei']
plt.rcParams['axes.unicode_minus'] = False
class Kalman_Filter():
    def __init__(self):
        self.A = np.array([[1, 1],
                           [0, 1]]) #A矩阵
        self.C = np.array([[1, 0],
                           [0, 1]]) #C矩阵

        self.mean = np.array([0, 0]) #随机变量u和v的期望

        self.Q = np.array([[0.1, 0],
                          [0, 0.1]]) #随机变量u的协方差矩阵，设定的值小一些，说明匀速直线运动更可信，并且假设相互独立
        self.R = np.array([[1, 0],
                          [0, 1]]) #随机变量v的协方差矩阵，说明GPS的可信度小，也假设相互独立
        z_0 = np.array([[0],
                        [1]]) #给定初始位置为0，速度为1 的 z1
        self.t=20 #假设要计算20个时刻对应的值
        self.z=np.zeros(shape=(self.t,2)) #初始化一个矩阵z，用于储存直线运动的所得的数据
        self.x=np.zeros(shape=(self.t,2)) #初始化一个矩阵x，用于储存GPS定位所得的数据
        for i in range(self.t): #迭代十次
            #计算新的值，并且加上随机变量，随机数从多维正态分布中抽样出一个数据加上去。reshape(-1,1)作用是将其变成二维数据
            z_0=self.A @ z_0 + stats.multivariate_normal.rvs(mean=self.mean,cov=self.Q).reshape(-1,1)
            x_0=self.C @ z_0 + stats.multivariate_normal.rvs(mean=self.mean,cov=self.R).reshape(-1,1)

            self.z[i, :] = z_0.squeeze()  # 将计算出来的直线运动的值储存，squeeze()作用是将z_0从二维数据变成一位
            self.x[i, :] = x_0.squeeze()  # 将计算出来的GPS的值储存
    def train(self):
        predict_μ =  np.array([0, 0]) #初始化P(x1)的参数，设置期望为0
        predict_Σ = np.array([[1, 0],
                            [0, 1]])#协方差矩阵对角线初始化为1

        result=np.zeros(shape=(self.t,2)) #用于储存计算出来的结果
        for i in range(self.t):
            K=predict_Σ @ self.C.T @ np.linalg.inv(self.C @ predict_Σ @ self.C.T + self.R) #卡尔曼增益系数
            update_μ=predict_μ + K @ (self.x[i,:] - self.C @ predict_μ) #更新的期望值
            update_Σ=predict_Σ - K @ self.C @ predict_Σ #更新的协方差矩阵
            predict_μ=self.A @ update_μ #计算下一个时刻的预测期望值
            predict_Σ=self.A @ update_Σ @ self.A.T + self.Q #计算下一个时刻的预测协方差矩阵
            result[i,:]=update_μ #保存更新后的期望值
        plot_figure(self.x, self.z,result) #绘图
def plot_figure(x,z,new_x):
    #仅绘制速度的图片
    plt.plot(x[:,1],label="GPS",linewidth=3.0,marker="o")
    plt.plot(z[:,1],label="直线匀速",linewidth=3.0,marker="o")
    plt.plot(new_x[:,1],label="卡尔曼滤波轨迹",linewidth=3.0,marker="o")
    plt.legend()
    plt.show()
if __name__ == '__main__':
    kalman_filter=Kalman_Filter() #初始化
    kalman_filter.train() #训练

结束

以上，就是卡尔曼滤波在运动轨迹优化上的应用了。如有问题，还望指出，阿里嘎多