卡尔曼滤波器
一、高斯函数
1.1 介绍
一维高斯函数的表达式为 \(f\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \mu \right )^{2}}{\delta ^2}}\)，其中\(\mu\)为函数尖峰值，\(\delta\)为标准方差。
def f(mu, sigma2, x):
return 1/sqrt(2.*pi*sigma2) * exp(-.5*(x-mu)**2 / sigma2)
1.2 状态更新
假设像贝叶斯定理一样，两个高斯函数相乘，\(f_1\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \mu \right )^{2}}{\delta ^2}}\) 和 \(f_2\left ( x \right )=\frac{1}{\sqrt{2\pi \gamma ^{2}}}exp^{-\frac{1}{2}\frac{\left ( x - \nu \right )^2}{\gamma ^2}}\)，相乘后获得新的高斯函数\(f_3\left ( x \right )=\frac{1}{\sqrt{2\pi \delta ^{2'}}}exp^{-\frac{1}{2}\frac{\left ( x - {\mu}' \right )^{2}}{\delta ^{2'}}}\)，其中\(f_3\left ( x \right )\) 的波峰处于\(f_1\left ( x \right )\)，\(f_2\left ( x \right )\)两个尖峰之间，且更细高，一个相对确定的分布，计算公式为\({\mu}'= \frac{1}{\delta ^2+\gamma ^2}[\gamma ^2\mu + \delta ^2\nu ]\)，\(\delta^{2'}= \frac{1}{\frac{1}{\delta ^2}+\frac{1}{\gamma ^2}}\)。
图1. 高斯函数
def update(mean1, var1, mean2, var2):
new_mean = 1/(var1+var2)*(var1*mean2+var2*mean1)
new_var = 1/(1/var1 + 1/var2)
return [new_mean, new_var]
1.3 高斯运动预测
预测函数是两个函数的相加，\(\mu '= \mu + \nu\)，\(\delta^{2'}= \delta ^2+\gamma ^2\)。
def predict(mean1, var1, mean2, var2):
new_mean = mean1 + mean2
new_var = var1 + var2
return [new_mean, new_var]
二、卡尔曼滤波
2.1 介绍
卡尔曼滤波器本质上是一个预测和更新的估算问题，根据之前一维高斯函数的观测状态\(f_2\)分布，与预测状态\(f_1\)分布，获得当前状态\(f_3\)分布，这种可以由两种相对不确定分布推导相对确定分布，是卡尔曼滤波的核心思想。
2.2 理论
下面是卡尔曼滤波器抽象出的7个步骤，
\(x\)表示滤波器的状态向量，\(F\)是状态转移矩阵，\({x}' = Fx+u\)，表示了状态 \(x\to x^{'}\) 的过程，是前一个状态对当前状态的预测。\(P\)是状态协方差矩阵，\(Q\)是过程噪声，\(P^{'}=FPF^{T}+Q\) 表示了 \(P\to P^{'}\) 的过程，是前一个不确定程度对当前状态的不确定程度的预测。
\(z\)表示观测状态量，\(H\)是测量矩阵，\(y=z-H{x}'\)表示了观测值和与预测值的差值。\(R\)是测量噪声矩阵，\(K\)为卡尔曼增益，是状态更新中的权重，在\(x={x}'+Ky\)中是y的权值，而在\(P=(I-KH){P}'\)中是\({P}'\)的权值。
卡尔曼滤波器的最后两个步骤是滤波器的闭环。\(x={x}'+Ky\)表示当前状态向量\(x\)的更新，\(P=(I-KH){P}’\)表示系统的不确定度\(P\)的更新，这些都被用于下一个周期的运算。
2.3 实例（一维追踪）
假设在恒定速度\(v\)的前提下，预测物体的位置\(s\)，这是一维卡尔曼滤波器的追踪问题，其中状态向量x为 \([s,v]^{T}\)，因为v保持不变，则当\(\Delta t = 1s\)时，\(F\)表示为\([[1., 1.], [0, 1.]]\)，当前预测的状态为 \({x}'\)，\(P\)的初始值可以设置为\([[1000., 0.], [0., 1000.]]\)，意味着速度和位置是两个不相关的变量，因为观测值只有\(s\)，则\(H\)为\([[1., 0.]]\)。因为实例是简化的模型，所以\(Q\)和\(R\)都设置为0，一般情况下，\(Q\)是那些\(P\)无法表示的噪声，而\(R\)由传感器厂家提供。
# 卡尔曼滤波，对当前位置的估计
def kalman_filter(x, P):
for n in range(len(measurements)):
# measurement update
z = matrix([[measurements[n]]])
y = z - H*x
S = H*P*H.transpose() + R
K = P*H.transpose()*S.inverse()
x = x + (K*y)
P = (I - (K*H))*P
# prediction
x = (F*x) + u
P = F*P*F.transpose()
return x,P
measurements = [1, 2, 3]
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
print(kalman_filter(x, P))
# output should be:
# x: [[3.9996664447958645], [0.9999998335552873]]
# P: [[2.3318904241194827, 0.9991676099921091], [0.9991676099921067, 0.49950058263974184]]
2.4 实例（二维追踪）
假设传感器为激光雷达，可以提供二维测量数据，首先要定义状态转移矩阵\(F\)，来估算二维位置\((s_x, s_y)\)和二维速度\((v_x,v_y)\)，则当\(\Delta t = 1s\)时，\(F\)表示为\([[1, 0,1,0], [0, 1,0,0,1],[0,0,1,0],[0,0,0,1]]\)，当前预测的状态为 \(x^{'}\)，又因为激光雷达只能测量距离，则对应的测量矩阵\(H=[[1,0,0,0],[0,1,0,0]]\)，而只能测量距离不能测量速度的性质，体现在协方差矩阵上就是位置测量的不确定度较低，速度测量的不确定度较高，则\(P = [[1,0,0,0],[0,1,0,0],[0,0,1000,0],[0,0,0,1000]]\)。该处理方式和一维追踪中的相同。
三、扩展卡尔曼滤波
3.1 介绍
扩展卡尔曼滤波是为了解决不同传感器融合时采集数据不统一的问题，比如激光雷达的观测值是在笛卡尔坐标系的，而毫米波雷达的观测值是在极坐标系的，在进行传感器融合时，需要统一坐标系，但是当坐标系转换时会引入非线性变化，于是这种非线性的卡尔曼滤波就称为扩展卡尔曼滤波。
3.2 毫米波雷达
毫米波雷达的原理是多普勒效应，其测量的数据都是在极坐标系下的，状态向量x为\([\rho ,\phi, \dot{\rho } ]^T\)。\(\rho\)为目标在极坐标下离雷达的距离，\(\phi\)为方向角，\(\dot{\rho }\)为距离的变化率。
因为预测的状态向量\({x}'\)为 \([{s_x}', {s_y}', {v_x}',{v_y}']^T\)，观测值和与预测值的差值的非线性的模型时，习惯于将计算差值\(y=z-H{x}'\)中的\(H{x}'\)用\(h({x}')\)表示。此时\(H{x}'\)不在是常数矩阵，高斯分布在经过非线性函数的变换后，其结果将不再符合高斯分布，这直接导致卡尔曼滤波器失效。因此我们需要将\(h({x}')\)先转化为近似的线性函数。\(y=h(x)\)可通过泰勒公式在点\((x_0,y_0)\)处展开为泰勒级数，并忽略二次以上的高阶项，于是可以如下的式子，
\(h(x)\approx h(x_0)+\dot{h}(x_0)(x-x_0)\)
其中，\(\dot{h}(x_0)\) 表示\(h(x)\)在\(x_0\)处对\(x\)的偏导，即\(\dot{h}(x_0) =\frac{\partial h(x_0)}{\partial x}\)，这一项被称为雅可比（Jacobian）式。
求得非线性函数\(h(x')\)对\({s_x}'\)，\({s_y}'\)，\({v_x}'\)，\({v_y}'\)的一阶偏导数，并排列成的矩阵，最终得到雅克比（Jacobian）矩阵\(H_{3\times 4 }\)，将\(\rho ,\phi, \dot{\rho }\)带入上述公式可得实际的测量矩阵\(H_{3\times 4 }\)，其余是卡尔曼滤波的标准步骤。
四、总结
扩展卡尔曼滤波，只是增加了对非线性观察矩阵H的线性化处理，依此来满足高斯分布的性质，即高斯分布在经过预测和测量值更新后，还是符合高斯分布。
卡尔曼滤波器的这些性质适合融合多个传感器，以此减少单个传感器的误差，降低了单个传感器的精度成本。
