理论知识
状态空间表达式
已知系统的状态方程为:
x
k
=
A
x
k
−
1
+
B
u
k
+
w
k
x_{k} =Ax_{k-1}+Bu_{k}+w_{k}
xk=Axk−1+Buk+wk
系统的观测方程为:
y
k
=
C
x
k
+
v
k
y_{k} =Cx_{k}+v_{k}
yk=Cxk+vk
故系统的结构图为:
其中,系统的模型误差为:
w
k
∼
N
(
0
,
Q
k
)
w_{k}\sim N(0,Q_{k} )
wk∼N(0,Qk)
系统的观测误差为:
v
k
∼
N
(
0
,
R
k
)
v_{k}\sim N(0,R_{k} )
vk∼N(0,Rk)
卡尔曼滤波
卡尔曼滤波是使用上一次的最优结果来预测当前的值,同时使用观测值修正当前的值,得到最优的结果。
卡尔曼滤波的预测公式为:
x
^
t
−
=
F
x
^
t
−
1
+
B
u
t
−
1
\hat{x} _{t}^{-} =F\hat{x} _{t-1}+B{u} _{t-1}
x^t−=Fx^t−1+But−1
P t − = F P t − 1 F T + Q P _{t}^{-} =FP _{t-1}F^{T}+Q Pt−=FPt−1FT+Q
卡尔曼滤波的更新公式为:
k
t
=
P
t
−
H
T
H
P
t
−
H
T
+
R
k_{t}=\frac{P_{t}^{-}H^{T}}{HP_{t}^{-}H^{T}+R}
kt=HPt−HT+RPt−HT
x ^ t = x ^ t − + k t ( z t − H x ^ t − ) \hat{x} _{t}=\hat{x} _{t}^{-}+k_{t}(z_{t}-H\hat{x} _{t}^{-}) x^t=x^t−+kt(zt−Hx^t−)
P t = ( I − k t H ) P t − P_{t}=(I-k_{t}H)P_{t}^{-} Pt=(I−ktH)Pt−
实践测试
背景假设
假定有一个小车正在进行匀速直线运动,其加速度为1米每二次方秒,测量频率为10Hz,即每0.1秒测量一次,小车的初始速度为0,使用卡尔曼滤波对小车的位置进行处理。
小车位置
小车理论位置
根据物理中的运动学公式,可以求得小车的位置,求得的小车位置和时间关系为:
小车实际位置
在小车实际运动以及小车实际位置获取时,小车的运动存在误差,包括小车运动时产生的误差和小车观测时产生的误差,故小车实际位置和时间关系为:
卡尔曼滤波
滤波过程
定义系统的状态量为:
x
=
[
p
o
s
v
]
x=\begin{bmatrix} pos\\v\end{bmatrix}
x=[posv]
其中,pos为小车的位置,v为小车的速度。
定义系统的采样时间为dt,单位为秒;定义系统的加速度为a,单位为米每二次方秒;定义系统的观测误差为R;定义系统的运行误差的方差为:
Q
=
[
Q
p
o
s
0
0
Q
v
]
Q=\begin{bmatrix} Q_{pos}& 0\\0 &Q_{v}\end{bmatrix}
Q=[Qpos00Qv]
系统的先验值为:
x
^
t
−
=
[
1
d
t
0
1
]
⋅
x
t
−
1
+
[
1
2
×
d
t
2
d
t
]
⋅
a
\hat{x}_{t}^{-}=\begin{bmatrix} 1& dt\\ 0&1\end{bmatrix}\cdot {x}_{t-1}+\begin{bmatrix}\frac{1}{2}\times dt^{2} \\dt\end{bmatrix}\cdot a
x^t−=[10dt1]⋅xt−1+[21×dt2dt]⋅a
更新误差协方差:
P
t
−
=
[
1
d
t
0
1
]
⋅
P
t
⋅
[
1
0
d
t
1
]
+
[
Q
p
o
s
0
0
Q
v
]
P_{t}^{-}=\begin{bmatrix} 1& dt\\ 0&1\end{bmatrix}\cdot P_{t} \cdot\begin{bmatrix} 1& 0\\ dt&1\end{bmatrix}+\begin{bmatrix} Q_{pos}& 0\\ 0&Q_{v}\end{bmatrix}
Pt−=[10dt1]⋅Pt⋅[1dt01]+[Qpos00Qv]
在获取测量值时,实际使用直接读取传感器的输出值,在本测试中读取来自测量值矩阵的数据。在本次测试中,测量值公式写为:
z
(
t
)
=
p
o
s
=
[
1
0
]
⋅
[
p
o
s
v
]
+
V
z(t)=pos=\begin{bmatrix}1 &0\end{bmatrix}\cdot \begin{bmatrix} pos\\v\end{bmatrix} +V
z(t)=pos=[10]⋅[posv]+V
卡尔曼滤波系数为:
k
t
=
P
t
−
⋅
[
1
0
]
[
1
0
]
⋅
P
t
−
⋅
[
1
0
]
+
R
k_{t}=\frac{P_{t}^{-}\cdot \begin{bmatrix} 1\\0\end{bmatrix}}{\begin{bmatrix}1 & 0\end{bmatrix} \cdot P_{t}^{-}\cdot \begin{bmatrix} 1\\0\end{bmatrix}+R}
kt=[10]⋅Pt−⋅[10]+RPt−⋅[10]
由此可以获得卡尔曼预测值为:
x
^
t
=
x
^
t
−
+
k
t
[
z
(
t
)
−
[
1
0
]
⋅
x
^
t
−
]
\hat{x}_{t} =\hat{x}_{t}^{-}+k_{t}[z(t)-\begin{bmatrix} 1& 0\end{bmatrix}\cdot \hat{x}_{t}^{-} ]
x^t=x^t−+kt[z(t)−[10]⋅x^t−]
最后更新协方差为:
P
t
=
[
[
1
0
0
1
]
−
k
t
⋅
[
1
0
]
]
⋅
P
t
−
P_{t}=[\begin{bmatrix} 1& 0\\ 0&1\end{bmatrix}-k_{t}\cdot \begin{bmatrix} 1&0\end{bmatrix}]\cdot P_{t}^{-}
Pt=[[1001]−kt⋅[10]]⋅Pt−
滤波结果
卡尔曼滤波的结果为:
执行结果
卡尔曼滤波执行的最终结果为