第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-创新互联
分享网址:http://pwwzsj.com/article/cssdie.html