来我们先来看一下第一眼看上去不知道在讲啥的公式
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滤波数据,记得刚看时很痛苦，现在回过头来看发现也就那麽回事，现在我先把这几
个式子改一下。
在五个公式之前先写一下预测方程和观测方程
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