经典的EKF公式

来我们先来看一下第一眼看上去不知道在讲啥的公式
1 x k − = A x k − 1 + B u k − 1 x_k^-=Ax_{k-1}+B_{u_{k-1}} xk−=Axk−1+Buk−1
2 P k − = A P k − 1 A T + Q P^-_k=AP_{k-1}A^T+Q Pk−=APk−1AT+Q
3 K k = P k − H T H P k − H T + R K_k=\frac{P^-_kH^T}{HP^-_kH^T+R} Kk=HPk−HT+RPk−HT
4 x k = x k − + K k ( y k − H x k − ) x_k=x^-_k+K_k(y_k-Hx_k^-) xk=xk−+Kk(yk−Hxk−)
5 P k = ( I − K k H ) P k − P_k=(I-K_kH)P^-_k Pk=(I−KkH)Pk−

然后在网上看到的资料解释---系统状态矩阵，状态转移矩阵，状态观测矩阵，过程噪声...各种专业名词一来—
我表示，我只想快速的运用它，能不能搞简单些!这个经典公式网上很多解释我就不做赘述，不了解的可以看
文章末尾的参考资料。

之前跟着大佬搞过一次EKF滤波数据,记得刚看时很痛苦，现在回过头来看发现也就那麽回事，现在我先把这几
个式子改一下。

简化版的EKF公式

在五个公式之前先写一下预测方程和观测方程
prediction x = A x k − 1 x=Ax_{k-1} x=Axk−1
observation y = H x k + u k y=Hx_k+u_k y=Hxk+uk
1 x k − = A x k − 1 x_k^-=Ax_{k-1} xk−=Axk−1
2 P k − = A P k − 1 A T + Q P^-_k=AP_{k-1}A^T+Q Pk−=APk−1AT+Q
3 K k = P k − H T H P k − H T + R K_k=\frac{P^-_kH^T}{HP^-_kH^T+R} Kk=HPk−HT+RPk−HT
4 x k = x k − + K ( y − H x k − ) x_k=x^-_k+K(y-Hx_k^-) xk=xk−+K(y−Hxk−)
5 P k = P k − − K k ( H P k − H T + R ) K k T P_k=P^-_k-K_k(HP^-_kH^T+R)K^T_k Pk=Pk−−Kk(HPk−HT+R)KkT

好像看着还是比较烦，但其实我们梳理一下
在EKF循环过程中我们需要知道的就是 A,H,P这三个东西，为了便于理解现在不考虑误差
那末我们的观测和预测方程就可以写成这样
prediction x = A x k − 1 x=Ax_{k-1} x=Axk−1
observation y = H x k y=Hx_k y=Hxk
一般情况A和H都是矩阵
举个简单例子来说明这两个矩阵，一个小车在已知坐标系xoy中随意运行，小车自身的状态有位置[x,y]，速度v,朝向 δ \delta δ,在这个坐标系中其他任何物体都有 [ x , y , δ ] [x,y,\delta] [x,y,δ],每个物体有在xoy坐标系中的坐标和车的坐标系 x c o c y c x_co_cy_c xcocyc的坐标;

现在假设小车上一时刻的状态为{ x 0 , y 0 , δ 0 x_0,y_0,\delta_0 x0,y0,δ0},角速度为a速度为 v v v,变化时间为 ∇ \nabla ∇t
那我们可以得到下一时刻的状态
F ( x , y , δ ) = { x = x 0 + v ∗ ∇ t ∗ cos ⁡ δ y = y 0 + v ∗ ∇ t ∗ sin ⁡ δ δ = δ 0 + a ∗ ∇ t F(x,y,\delta)=\left\{ \begin{array}{rcl} x = & {x_0+v*\nabla t*\cos \delta}\\ y =& {y_0+v*\nabla t*\sin \delta}\\ \delta =&{\delta_0+a*\nabla t} \end{array} \right. F(x,y,δ)=⎩⎨⎧x=y=δ=x0+v∗∇t∗cosδy0+v∗∇t∗sinδδ0+a∗∇t
⇓ F ( X ) \Downarrow F(X) ⇓F(X)
∣ 1 0 − v ∗ ∇ t ∗ sin ⁡ δ 0 1 v ∗ ∇ t ∗ cos ⁡ δ 0 0 1 ∣ \left| \begin{array}{ccc} \\1 & 0 & -v*\nabla t*\sin \delta \\ 0 & 1 &v*\nabla t*\cos \delta \\ 0 & 0 & 1\end{array} \right| ∣∣∣∣∣∣∣∣100010−v∗∇t∗sinδv∗∇t∗cosδ1∣∣∣∣∣∣∣∣

F(X)==A
F(X)的三列是分别对x,y, v v v求偏导得到。
接下来我们接着求H,我们首先先列出H的表达是，这里还是需要强调一下H是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系

接着上面例子来计算，在观测的时候其他物体都是相对车来表达的也就是说观测的数据是一个相对位置的坐标，而我们使用的状态是要在xoy里的，那 H 就是 x c o c y c x_co_cy_c xcocyc到xoy的一个转换关系。 [ x w , y w ] [x_w,y_w] [xw,yw]是物体在xoy的坐标， [ x c , y c ] [x_c,y_c] [xc,yc]是车在 x o y xoy xoy坐标。
h ( x , y , δ ) = ∣ cos ⁡ δ c sin ⁡ δ c − sin ⁡ δ c cos ⁡ δ c ∣ ∣ x w − x c y w − y c ∣ h(x,y,\delta)=\left| \begin{array}{cc} \cos\delta_c & \sin\delta_c \\ -\sin\delta_c & \cos \delta_c\end{array} \right|\left| \begin{array}{cc} x_w-x_c \\ y_w-y_c\end{array} \right| h(x,y,δ)=∣∣∣∣cosδc−sinδcsinδccosδc∣∣∣∣∣∣∣∣xw−xcyw−yc∣∣∣∣
⇓ H ( X ) \Downarrow H(X) ⇓H(X)
∣ − cos ⁡ δ c − sin ⁡ δ c − ( x w − x c ) s i n δ c + ( y w − y c ) c o s δ c c o s δ c s i n δ c 0 1 − ( x w − x c ) c o s δ c − ( y w − y c ) s i n δ c − s i n δ c c o s δ c ∣ \left| \begin{array}{ccc} -\cos\delta_c & -\sin\delta_c &-(x_w-x_c)sin\delta_c+(y_w-y_c)cos\delta_c & cos\delta_c &sin\delta_c \\ 0 & 1 &-(x_w-x_c)cos\delta_c-(y_w-y_c)sin\delta_c&-sin\delta_c& cos\delta_c \end{array} \right| ∣∣∣∣−cosδc0−sinδc1−(xw−xc)sinδc+(yw−yc)cosδc−(xw−xc)cosδc−(yw−yc)sinδccosδc−sinδcsinδccosδc∣∣∣∣

H(X)=H
现在我们就知道了H 和 A那我们就可以开心的套公式了。哦，还有个P ，这是啥呢？官方语言叫协方差，是用来表示我们数据的关联程度的，中间过程用，关于P和剩下的公式套用下节讲。

参考资料

1 博文https://blog.csdn.net/woaizgw/article/details/73648578
2 博文https://blog.csdn.net/fzch_struggling/article/details/44998901
3博文https://blog.csdn.net/wccsu1994/article/details/84643221

简单的理解EKF算法1相关推荐

简单的理解deflate算法
先说一下deflate算法吧. deflate是zip压缩文件的默认算法. 其实deflate现在不光用在zip文件中, 在7z, xz等其他的压缩文件中都用. 实际上deflate只是一种 ...
几种经典非线性滤波算法简单概括（EKF，UKF，CKF，PF）
几种经典非线性滤波算法概括(EKF,UKF,CKF,PF) 上一篇文章阐述了Kalman滤波算法,该算法是在线性高斯下的最优滤波估计算法.但是在实际控制系统中,系统的动态过程和测量过程很多情况都是非线 ...
直观地简单理解Tarjan算法（寻找有向图中的强连通图）
简单理解Tarjan算法按照百度百科的播报应该是读成['ta:rdʒən]?看过了几篇网络上的解释虽然都讲得比较具体但刚开始都难以理解,所以打算写一个更直观的理解方式. Tarjan算法是求 ...
用人话理解hash算法、特性、唯一性等
背景博主在本科的时候曾经参加过数学建模,当初提交参赛论文的时候有两个提交时间点,第一个是提交MD5码的时间,第二个是提交具体参赛论文的时间,当初就很纳闷,直接提交论文不就ok吗?为什么多此一举呢我 ...
xgboost 正则项_深入理解Boosting算法(4)-XGBoost
导语:记得第一次使用XGBoost的时候还是2016年,那会的XGBoost工具还不完善,深度模型还没有怎么样火起来,大家用的最多的还是Sklearn里面的GBDT,或者R语言的GBM.后来在工作中也 ...
从零实现来理解机器学习算法：书籍推荐及障碍的克服
从零实现来理解机器学习算法:书籍推荐及障碍的克服发表于2015-09-10 16:16| 1261次阅读| 来源Machine Learning Mastery| 2 条评论| 作者Jason ...
简单的加密/解密算法_/c++
版权声明:本文为博主原创文章,未经博主允许不得转载. https://blog.csdn.net/qq_36557960/article/details/79299093 关于加密和解密问题,有的加密 ...
通俗理解Paxos算法
维基的简介:Paxos算法是莱斯利·兰伯特(Leslie Lamport,就是 LaTeX 中的"La",此人现在在微软研究院)于1990年提出的一种基于消息传递且具有高度容错特性 ...
一步一步理解Paxos算法
一步一步理解Paxos算法作者:jw (360电商技术组) 背景 Paxos算法是Lamport于1990年提出的一种基于消息传递的一致性算法.由于算法难以理解起初并没有引起人们的重视,使Lampo ...

简单的理解EKF算法1

简单的理解EKF算法

经典的EKF公式

简化版的EKF公式

参考资料

简单的理解EKF算法1相关推荐

最新文章

热门文章