第11章TheExtendedKalmanFilter-创新互联
- Linear Kalman Filter, 状态方程
x ˙ = A x + B u + w z = H x + v \begin{aligned} \dot{\pmb{x}} &= \pmb{Ax}+\pmb{Bu}+\pmb{w} \\ \pmb{z} &= \pmb{Hx}+\pmb{v} \end{aligned} xxx˙zzz=AxAxAx+BuBuBu+www=HxHxHx+vvv
目前成都创新互联已为上1000家的企业提供了网站建设、域名、虚拟主机、网站托管、服务器托管、企业网站设计、昌都网站维护等服务，公司将坚持客户导向、应用为本的策略，正道将秉承"和谐、参与、激情"的文化，与客户和合作伙伴齐心协力一起成长，共同发展。- Extended Kalman Filter, 状态方程
x = f ( x , u ) + w z = h ( x ) + ν \begin{aligned} \pmb{x} &= \pmb{f}(\pmb{x},\pmb{u})+\pmb{w}\\ \pmb{z} &= \pmb{h}(\pmb{x})+\pmb{\nu} \end{aligned} xxxzzz=fff(xxx,uuu)+www=hhh(xxx)+ννν
F = ∂ f ( x t , u t ) ∂ x ∣ x t , u t H = ∂ h ( x ˉ t ) ∂ x ˉ ∣ x ˉ t \begin{aligned} \mathbf F &= {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}} \\ \mathbf H &= \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t} \end{aligned} FH=∂x∂f(xt,ut)∣∣∣∣xt,ut=∂xˉ∂h(xˉt)∣∣∣∣xˉt
linear Kalman filter EKF F = ∂ f ( x t , u t ) ∂ x ∣ x t , u t x ˉ = F x + B u x ˉ = f ( x , u ) P ˉ = F P F T + Q P ˉ = F P F T + Q H = ∂ h ( x ˉ t ) ∂ x ˉ ∣ x ˉ t y = z − H x ˉ y = z − h ( x ˉ ) K = P ˉ H T ( H P ˉ H T + R ) − 1 K = P ˉ H T ( H P ˉ H T + R ) − 1 x = x ˉ + K y x = x ˉ + K y P = ( I − K H ) P ˉ P = ( I − K H ) P ˉ \begin{array}{l|l} \text{linear Kalman filter} & \text{EKF} \\ \hline & \boxed{\mathbf F = {\frac{\partial{f(\mathbf x_t, \mathbf u_t)}}{\partial{\mathbf x}}}\biggr|_{{\mathbf x_t},{\mathbf u_t}}} \\ \mathbf{\bar x} = \mathbf{Fx} + \mathbf{Bu} & \boxed{\mathbf{\bar x} = f(\mathbf x, \mathbf u)} \\ \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q & \mathbf{\bar P} = \mathbf{FPF}^\mathsf{T}+\mathbf Q \\ \hline & \boxed{\mathbf H = \frac{\partial{h(\bar{\mathbf x}_t)}}{\partial{\bar{\mathbf x}}}\biggr|_{\bar{\mathbf x}_t}} \\ \textbf{y} = \mathbf z - \mathbf{H \bar{x}} & \textbf{y} = \mathbf z - \boxed{h(\bar{x})}\\ \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} & \mathbf{K} = \mathbf{\bar{P}H}^\mathsf{T} (\mathbf{H\bar{P}H}^\mathsf{T} + \mathbf R)^{-1} \\ \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} & \mathbf x=\mathbf{\bar{x}} +\mathbf{K\textbf{y}} \\ \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} & \mathbf P= (\mathbf{I}-\mathbf{KH})\mathbf{\bar{P}} \end{array} linear Kalman filterxˉ=Fx+BuPˉ=FPFT+Qy=z−HxˉK=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)PˉEKFF=∂x∂f(xt,ut)∣∣∣∣xt,utxˉ=f(x,u)Pˉ=FPFT+QH=∂xˉ∂h(xˉt)∣∣∣∣xˉty=z−h(xˉ)K=PˉHT(HPˉHT+R)−1x=xˉ+KyP=(I−KH)Pˉ
11.2 实例
背景：1）考虑二维平面；2）Aircraft只有水平方向的速度；3）Aircraft高度假设不变；4）Radar随着距离的变大，精度减低
- 下面这个是Radar的模型：
class Radar:
"""模拟雷达传感器(二维平面)"""
def __init__(self, dt, pos, vel, alt):
"""
Parameters
----------
dt : double
采样时间
pos : double
水平距离
vel : double
水平速度
alt : double
垂直高度
Returns
-------
None.
"""
# 初始化Radar
self.dt = dt
self.pos = pos
self.vel = vel
self.alt = alt
def get_range(self):
"""得到物体当前运动信息"""
# 添加一些噪声
self.vel = self.vel + .1*randn()
self.alt = self.alt + .1*randn()
# 从上一刻的位置获得当前时刻的位置
self.pos = self.pos + self.vel*self.dt
# 假设雷达的精度随着距离变远而越来越低
err = self.pos * (0.05*randn())
dist = math.sqrt(self.pos**2 + self.alt**2) + err
return dist下面说明系统的状态方程和测量方程，首先得确定系统的状态变量 x = ( x 0 x 1 x 2 ) T \pmb{x} = \begin{pmatrix} x_0 & x_1 & x_2 \end{pmatrix}^T xxx=(x0x1x2)T
x 0 {x}_0 x0：水平距离。 x 1 {x}_1 x1：水平速度。 x 2 {x}_2 x2：垂直高度。
- 系统状态方程：（需要注意，速度只有水平方向，所以 x ˙ 0 = x 1 \dot{x}_0=x_1 x˙0=x1）
[ x ˙ 0 x ˙ 1 x ˙ 2 ] = [ 0 1 0 0 0 0 0 0 0 ] [ x 0 x 1 x 2 ] + w \begin{bmatrix} \dot{x}_0 \\ \dot{x}_1 \\ \dot{x}_2 \\ \end{bmatrix} =\begin{bmatrix} 0 & 1 & 0 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \\ \end{bmatrix} \begin{bmatrix} x_0 \\ x_1 \\ x_2 \\ \end{bmatrix} + \pmb{w} ⎣⎡x˙0x˙1x˙2⎦⎤=⎣⎡000100000⎦⎤⎣⎡x0x1x2⎦⎤+www
系统的状态方程是连续的线性的，将其离散化（对确定性部分）：
F = e A d t ≈ I + A d t \begin{aligned} \pmb{F} &= e^{\pmb{A}dt} \\ &\approx \pmb{I} + \pmb{A}dt \end{aligned} FFF=eAAAdt≈III+AAAdt
所以离散化后的状态方程有：(其实这里还有一个问题， w \pmb{w} www变成什么了？）
x ( k ) = [ 1 d t 0 0 1 0 0 0 1 ] x ( k − 1 ) \pmb{x}(k)= \begin{bmatrix} 1 & dt & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \pmb{x}(k-1) xxx(k)=⎣⎡100dt10001⎦⎤xxx(k−1)
- 测量方程
z = h ( x ) + v = x 0 2 + x 2 2 + v \begin{aligned} \pmb{z} &= \pmb{h}(\pmb{x}) + \pmb{v} \\ &= \sqrt{x_{0}^{2}+x_{2}^{2}} + \pmb{v} \\ \end{aligned} zzz=hhh(xxx)+vvv=x02+x22 +vvv
离散化后，测量方程为；
z k = [ x 0 x 0 2 + x 2 2 0 x 2 x 0 2 + x 2 2 ] x k \pmb{z}_k= \begin{bmatrix} \frac{x_0}{x_0^2+x_2^2} & 0 & \frac{x_2}{x_0^2+x_2^2} \end{bmatrix} \pmb{x}_k zzzk=[x02+x22x00x02+x22x2]xxxk
这里有几点需要注意：
- 特别是随机噪声 Q , R \pmb{Q},\pmb{R} QQQ,RRR，需要根据实际的情况进行不断的调整
- 初始值的设定具有很大的随机性， x 0 , P 0 \pmb{x}_0,\pmb{P}_0 xxx0,PPP0几乎没有什么影响。
关于这一点可以参考一篇文献，严恭敏老师的《传统组合导航中的实用Kalman滤波技术评述》，给出了很多实用的建议：1）只要滤波器是渐近稳定，初值不会影响估计的最终结果（和系统的可观性有一定的关系）；2）去精确的离散化噪声的协方差矩阵，还不如增加采样频率所提高的精度多。
- 仿真结果（及其调试的过程记录）
- 协方差矩阵的更新公式，在代码中的实现出现了错误。/_ \
- 然后改到正确的公式后，它竟然还是发散的X_X
- 协方差矩阵的更新公式，在代码中的实现出现了错误。/_ \
问题分析：1）在初始值的估计值还是没有偏差的情况下进行的。。2）
解决办法：1）查看kalman滤波是否正确表达了；2）将初始值调为有一定偏差的，看是否有其它的问题；3)增加计算的时间后，又出现了NaN的数据，看来电脑的计算还是没有搞清楚！
结果：发现对于Numpy的数值的维度有点混乱，导致矩阵的计算出现了些许问题。现在已经有点晕了，需要重新再来一遍（遇到了一点问题，于是就又想逃避了哈？？还是先吃饭吧）
(python numpy) np.array.shape 中 (3,)、(3,1)、(1,3)的区别_我是一颗棒棒糖的博客-博客_np.array.shape
这里在强调一下，需要注意一下这个的运算过程，看看是否对于np.dot有什么误解呢。（突然好想用matlab啊，这个python是真的┭┮﹏┭┮）
numpy还有数组和矩阵的区别，我是真的醉醉的呢啊，下面我该怎么办？(就用数组类型了，现在就是要搞懂数组的运算，当然刚才维数这里也搞错了一点问题，继续加深理解呗）
- 经过仔细的调整，主要就是numpy的narray的运算进行了详细的对比，现在的结果稍微好了一点点呢！（这里对于状态的初始值也进行了调整和容易看出来的）
现在这里还有一个很明显的问题，速度的收敛速度好像有点慢，而且最后对于高度的估计有发散的趋势吗？（延长一点时间看看？）
然后它它，就直接起飞！（分析原因，因为随着距离的增加，radar的噪声逐渐增大的，然后就不能正确的估计啦？后面调试后确实是的）
一个这个程序，竟然调了两天，我？？？
分析原因：
- EKF还是不太熟悉，连公式都打错
- 第一次用Python来写，对于一些基本的运算不太了解，导致很多错误的都是计算的形式有问题
- 遇到问题的心态要好好调整，哪里不会就点哪个（人生苦短，不用什么都要学透）
Kalman-and-Bayesian-Filters-in-Python
你是否还在寻找稳定的海外服务器提供商？创新互联www.cdcxhl.cn海外机房具备T级流量清洗系统配攻击溯源，准确流量调度确保服务器高可用性，企业级服务器适合批量采购，新人活动首月15元起，快前往官网查看详情吧
网站题目：第11章TheExtendedKalmanFilter-创新互联
URL链接：http://mswzjz.cn/article/cssdie.html