从贝叶斯原理看卡尔曼滤波；公式解读；行人状态追踪(附代码)

概率论知识，感觉博主写的很到位了。

指路--->https://blog.csdn.net/varyshare/article/details/97614323

1.联合概率、条件概率

a.联合概率，通俗来说，就是既A又B的概率，比如100个人里面，有40个人长头发，有70个人涂口红，那么从人群众选一个人，既涂口红又长头发的概率就是 $\frac{40}{100}\times\frac{70}{100}= \frac{25}{7}$ ,称为联合概率，记作 $P(AB)$ , $P(A,B)$ . 其中的， $\frac{40}{100},\frac{70}{100}$ 是边缘概率，记作 $P(A),P(B)$ .

b.条件概率,根据概率论与数理统计，高等教育出版社，条件概率一节中，定义1.4

A,B是两个事件，且P(A)>0,称 $\frac{P(AB)}{P(A)}$ 为在事件A发生的条件下事件B发生的条件概率，记为P(B|A)，记有

$P(A|B)=\frac{P(AB)}{P(A)}$ 。

2.贝叶斯公式

$P(B|A)=\frac{P(AB)}{P(B)}$ 推得 $P(AB)=P(A)\timeP(B|A)$ 推得 $P(A|B)=\frac{P(B|A)P(A)}{P(B)}$

P(A)称为先验概率，P(B|A)似然概率，P(A|B)后验概率。

A为事件留长发，B为事件是女人。

已知人群中留长发的概率，P(A);人群中女性的占比P(B);留长发人中女人的比例为P(B|A),那么已知该人是一名女性，则她留长发的概率为P(A|B).可通过贝叶斯公式求得。

P(ABC)=P(C|AB)P(AB)=P(C|AB)P(B|A)P(A)

3.卡尔曼滤波的贝叶斯原理。

尤其是机器人状态估计的贝叶斯滤波

https://blog.csdn.net/varyshare/article/details/97642209

已知:

1. $P(z_{t}|x_{t})$ ，观测模型。这个表示的含义是：当机器人处于 $x_{t}$ 这个状态时，它观测值是 $z_{t}$ 的概率。这个是观测模型，需要我们自己建模，不同的传感器肯定是不一样的，比如我们设计一种函数 $f(x_{t},z_{t})$ 它的值就是 $P(z_{t}|x_{t})$ 。这个就是卡尔曼滤波要做的事，卡尔曼滤波设计的函数就是认为这个概率函数是一个正态分布（高斯分布）。

2. $P(x_{t}|u_{t},x_{t-1})$ 预测模型(处理模型).这个表示的含义是：当机器人上个状态是 $x_{t-1}$ ，然后它采取的动作（控制命令）是 $u_{t}$ 时，机器人的状态是 $x_{t}$ 的概率。这个是控制模型，也是需要我们自己建模不同的模型不一样。（卡尔曼滤波是认为这个控制模型是一个线性模型）
3. $P(u_{t}|x_{t-1})$ 这个表示的含义是：当机器人处于 $x_{t-1}$ 这个状态时做出 $u_{t}$ 的动作概率。

机器人状态估计中的假设：
假设当前观测值 $z_{t}$ 只与当前状态 $x_{t}$ 有关，跟机器人的控制命令 $u_{t}$ 和上个状态 $x_{t-1}$ 无关。
假设当前状态 $x_{t}$ 只与上个时刻状态 $x_{t-1}$ 以及与控制命令 $u_{t}$ 有关。

推导过程见上面博客链接

结论:

$P(x_{t}|z^{_{t}},u^{_{t}},x_{t-1})=\frac{P(z_{t}|x_{t})P(x_{t}|u_{t},x_{t-1})}{P(z_{t})}$

现在分子已经都是已知量， $P(z_{t})$ 是先验概率，是已知的，所以是个常数，相当于 $P(z_{t})$ 的高斯分布已知，然后 $x_{t}$ 对应的 $P(z_{t})$ 也是确定的。用 $\eta$ 表示。

故又可表示为： $P(x_{t}|z^{_{t}},u^{_{t}},x_{t-1})=\eta P(z_{t}|x_{t})P(x_{t}|u_{t},x_{t-1})$ 。

这就是我们在机器人状态估计中最常见的公式，无论是贝叶斯估计还是卡尔曼滤波都是这个公式。

4.卡尔曼滤波原理

参考了https://zhuanlan.zhihu.com/p/77327349

卡尔曼滤波解决两个问题：

1）传感器存在噪声的问题。比如，机器人状态估计问题。

2）用直接测量量表示需要的间接测量值，比如火箭的发动机舱温度不可直接测量得到，则用温度较低的部位的测量量推断得到。

实例描述：

导弹打击地方军事基地为例，可以通过传感器测量距离要打击的军事基地的距离为7km(测量值)，已知导弹的速度4km/s，上一秒估计的导弹距军事基地的距离为10km。则这一秒估计值为6km(预测模型或者处理模型)。

那么问题来了，导弹离目标的距离现在既有个观测值7km，又有个估计值6km。到底相信哪个？单纯相信观测值万一雷达被敌方干扰了呢？单纯相信估计值那么万一上个时刻的距离估计值或者速度不准呢？所以，我们要根据观测值和估计值的准确度来得到最终导弹离目标的距离估计值。准确度高的就最终结果比重高，准确度低就占比低。如果雷达测量的那个7m准确度是49%，根据速度估计出的那个6m准确度是1%，那么最终的距离估计结果就是 $7\times\frac{0.49}{0.49+0.01}+6\times(1-\frac{0.49}{0.49+0.01})$ , 其中， $\frac{0.49}{0.49+0.01}$ 为卡尔曼增益。它就是表示这个雷达数据相对于根据速度计算出的估计值的靠谱程度。

问题：1）传感器测量值7km，存在传感器噪声问题。

2）传感器估计值6km，存在速度存在噪声或者估计值不准的问题。

解决办法：求取加权平均值，引入卡尔曼滤波，用概率值表示数据的可信度。

1）认为 $x_{t-1}$ ， $v_{t-1}$ ， $z_{t}$ 均符合正态分布(高斯分布)，N(均值，方差)表示正态分布。

下面展示 $x_{t-1}$ 的正态分布，在均值为10时，其概率值最大。

$x_{t-1}=N(10,0.2^{2})$ ; $v_{t}=N(4,0.7^{2})$ ; $z_{t}=N(7,0.1^{2})$

2）求 $x_{t}$

a. $x_{t}=x_{t-1}-v_{t-1}=N(6,0.2^{2}+0.7^{2})$ 粗略估计值求取，预测模型。

b. $x_{t}=x_{t}\times z_{t}=N(6,0.2^{2}+0.7^{2})\times N(7,0.1^{2})$

$=N(6*0.1^{2}+7*0.728^2,0.728^2*0.1^2)$

(根据贝叶斯滤波公式的推导)。这里的 $x_{t}$ 时a中的估计值。 $0.2^2+0.7^2=0.728^2$

事实上我们就是把方差作为可信度，方差越大越不可信。然后用可信度做权重进行加权和得到均值。当然加权和的权重之和应该等于1，所以分母都除个 $0.728^2+0.1^2$ 。于是我们得到了当前时刻导弹位置的最优估计的概率分布：

$N(\frac{6*0.1^{2}+7*0.728^2}{0.728^2+0.1^2},\frac{0.728^2*0.1^2}{(0.728^2+0.1^2)^2})$

正态分布是在均值那个地方的概率最大。所以当前时刻导弹位置的最优估计就是上面这个概率分布的均值。

$N(\frac{6*0.1^{2}+7*0.728^2}{0.728^2+0.1^2},\frac{0.728^2*0.1^2}{(0.728^2+0.1^2)^2})$

$=N(\frac{0.1^{2}}{0.728^2+0.1^2}*6+\frac{0.728^2}{0.728^2+0.1^2} *7,\frac{0.728^2*0.1^2}{(0.728^2+0.1^2)^2})$

$=N((1-\frac{0.728^{2}}{0.728^2+0.1^2})*6+\frac{0.728^2}{0.728^2+0.1^2} *7,\frac{0.728^2*0.1^2}{(0.728^2+0.1^2)^2})$

其中， $\frac{0.728^2}{0.728^2+0.1^2}$ 为卡尔曼增益。

卡尔曼滤波公式：

特别好的推导。

https://blog.csdn.net/u010720661/article/details/63253509

Predict Part

$x_{k}=Fx_{k}+Bu_{k}$

$P_{k}=FP_{K}F^T+Q$

Update Part

$G_{k}=P_{k}H^{T}(HP_{k}H^{T}+R)^{-1}$

$x_{k}=x_{k}+G_{k}(z_{k}-Hx_{k})$

$P_{k}=(1-G_{k}H)P_{k}$

-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------

1.第一个公式各参数含义

$x_{k}$ 代表要估计的状态，比如 $x_{k}=[p_{x},p_{y},v_{x},v_{y}]^{T}$ ,其中p代表position

F代表预测矩阵，在这里面是 $\begin{bmatrix} 1& 0& \Delta t& 0\\ 0& 1& 0& \Delta t\\ 0& 0 & 1& 0\\ 0& 0 & 0& 1 \end{bmatrix}$

$p_{x}=p_{x}+\Delta tv_{x}$

$p_{y}=p_{y}+\Delta tv_{y}$

$v_{x}=v_{x}$

$v_{y}=v_{y}$

外部控制

B代表控制矩阵

u代表控制向量

比如，

$p_{x}=p_{x}+\Delta tv_{x}+\frac{1}{2}a_{x}\Delta t^{2}$

$p_{y}=p_{y}+\Delta tv_{y}+\frac{1}{2}a_{y}\Delta t^{2}$

$v_{x}=v_{x}+a_{x}\Delta t$

$v_{y}=v_{y}+a_{y}\Delta t$

此时，u是控制向量 $a=\begin{bmatrix} a_{x}\\ a_{y} \end{bmatrix}$

B为控制矩阵 $\begin{bmatrix} \frac{1}{2}\Delta t^{2} &0 \\ 0&\frac{1}{2}\Delta t^{2} \\ \Delta t&0 \\ 0& \Delta t \end{bmatrix}$

2.第二个公式各参数含义

协方差矩阵的含义：表示第(i,j)个元素之间的相关程度。对角线是格子元素的方差，协方差矩阵是对称矩阵

$P_{k}=FP_{K}F^T+Q$

$P_{k}$ 是协方差矩阵。

外部噪声的存在，比如空气阻力等

Q是噪声的协方差，为了补偿追踪不到的上述噪声。

这里补充知识，

$Cov(x)=\sum=P_{k}$

$Cov(Ax)=A\sum A^{T}$

3. $G_{k}=P_{k}H^{T}(HP_{k}H^{T}+R)^{-1}$

$G_{k}$ 代表卡尔曼滤波增益。也就是预测值和测量值根据误差大小计算得到的权重配比。

H就是测量矩阵，在这里能测到的是 $v_{x},v_{y}$ 。所以， $H=\begin{bmatrix} 0& 0 & 1 & 0\\ 0& 0 & 0 & 1 \end{bmatrix}$

R是传感器噪声的协方差矩阵。

4. $x_{k}=x_{k}+G_{k}(z_{k}-Hx_{k})$

$z_{k}$ 是传感器测量值。进行最优估计。

5. $P_{k}=(1-G_{k}H)P_{k}$

更新预测噪声

python,行人状态轨迹代码

来源自https://blog.csdn.net/AdamShan/article/details/78248421

import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import norm


# 1.初始化   状态量x,预测噪声矩阵P，间隔dt，预测矩阵、测量矩阵、测量噪声矩阵R，Q是处理噪声的协方差矩阵
x = np.matrix([[0.0, 0.0, 0.0, 0.0]]).T
print(x, x.shape)
P = np.diag([1000.0, 1000.0, 1000.0, 1000.0])
print(P, P.shape)

dt = 0.1 # Time Step between Filter Steps
F = np.matrix([[1.0, 0.0, dt, 0.0],
              [0.0, 1.0, 0.0, dt],
              [0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(F, F.shape)

H = np.matrix([[0.0, 0.0, 1.0, 0.0],
              [0.0, 0.0, 0.0, 1.0]])
print(H, H.shape)


ra = 0.09
R = np.matrix([[ra, 0.0],
              [0.0, ra]])
print(R, R.shape)

sv = 0.5
G = np.matrix([[0.5*dt**2],
               [0.5*dt**2],
               [dt],
               [dt]])
Q = G*G.T*sv**2
print(Q,Q.shape)

I = np.eye(4)
print(I, I.shape)

#2.产生随机测量数据
m = 200 # Measurements
vx= 20 # in X
vy= 10 # in Y

mx = np.array(vx+np.random.randn(m))
my = np.array(vy+np.random.randn(m))
measurements = np.vstack((mx,my))
print(measurements)
print(measurements.shape)
#np.std 求标准差
print('Standard Deviation of Acceleration Measurements=%.2f' % np.std(mx))
print('You assumed %.2f in R.' % R[0,0])

fig = plt.figure(figsize=(16,5))
plt.step(range(m),mx, label='$\dot x$')
plt.step(range(m),my, label='$\dot y$')
plt.ylabel(r'Velocity $m/s$')
plt.title('Measurements')
plt.legend(loc='best',prop={'size':18})

#3.保存数据
xt = []
yt = []
dxt= []
dyt= []
Zx = []
Zy = []
Px = []
Py = []
Pdx= []
Pdy= []
Rdx= []
Rdy= []
Kx = []
Ky = []
Kdx= []
Kdy= []

def savestates(x, Z, P, R, K):
    xt.append(float(x[0]))
    yt.append(float(x[1]))
    dxt.append(float(x[2]))
    dyt.append(float(x[3]))
    Zx.append(float(Z[0]))
    Zy.append(float(Z[1]))
    Px.append(float(P[0,0]))
    Py.append(float(P[1,1]))
    Pdx.append(float(P[2,2]))
    Pdy.append(float(P[3,3]))
    Rdx.append(float(R[0,0]))
    Rdy.append(float(R[1,1]))
    Kx.append(float(K[0,0]))
    Ky.append(float(K[1,0]))
    Kdx.append(float(K[2,0]))
    Kdy.append(float(K[3,0]))



# 4.卡尔曼滤波过程
for n in range(len(measurements[0])):

    # Time Update (Prediction)
    # ========================
    # Project the state ahead
    x = F*x

    # Project the error covariance ahead
    P = F*P*F.T + Q

    # Measurement Update (Correction)
    # ===============================
    # Compute the Kalman Gain
    S = H*P*H.T + R
    K = (P*H.T) * np.linalg.pinv(S)

    # Update the estimate via z
    Z = measurements[:,n].reshape(2,1)
    y = Z - (H*x)                            # Innovation or Residual
    x = x + (K*y)

    # Update the error covariance
    P = (I - (K*H))*P

    # Save states (for Plotting)
    savestates(x, Z, P, R, K)

EKF和UKF待续。。。

从贝叶斯原理看卡尔曼滤波；公式解读；行人状态追踪(附代码)

猜你喜欢