卡尔曼滤波1:状态空间方程
本文目的
结合例子,介绍卡尔曼算法的状态空间方程(观测方程和系统方程)为什么是这样建模的,准确的状态方程和观测方程建模,对卡尔曼算法至关重要。
前置知识点
- 基本线性代数知识
例子
现在有一个物体k时刻速度为
v
⃗
k
\vec{v}_k
vk,位置为
s
⃗
k
\vec{s}_k
sk,求的他各个时刻的速度和位置,把需要求解的内容作为系统状体
x
k
x_k
xk有:
x
k
=
[
s
k
v
k
]
x_k=\begin{bmatrix} s_k\\v_k\end{bmatrix}
xk=[skvk]
系统模型
这里的状态 x k x_k xk可以先简单理解为数据,结合一个例子逐步对系统模型进行完善。
1. 无外部输入的理想系统模型
1.1 无外部输入理想模型理论定义
一些线性系统可建模成如下形式,系统k时刻的状态
x
k
x_k
xk取决于k-1时刻系统状态
x
k
−
1
x_{k-1}
xk−1,且是一个线性关系,数学表达式为:
x
k
=
A
x
k
−
1
x_k = Ax_{k-1}
xk=Axk−1
当已知
x
k
−
1
x_{k-1}
xk−1就可以递推求出任意时刻的状态,是一个确定过程。
1.2 无外部输入理想模型理论例子分析
在该例子中,无外部输入的情况下,物体一直匀速直线运动,位移和速度可以有如下模型表征:
x
k
=
[
s
k
v
k
]
=
[
1
Δ
t
0
1
]
[
s
k
−
1
v
k
−
1
]
=
A
x
k
−
1
x_k=\begin{bmatrix} s_k\\v_k\end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\0 & 1 \end{bmatrix}\begin{bmatrix} s_{k-1}\\v_{k-1}\end{bmatrix}=Ax_{k-1}
xk=[skvk]=[10Δt1][sk−1vk−1]=Axk−1
也就是
x
k
=
A
x
k
−
1
x_k = Ax_{k-1}
xk=Axk−1
当已知
x
k
−
1
x_{k-1}
xk−1,也就是
s
k
−
1
s_{k-1}
sk−1和
v
k
−
1
v_{k-1}
vk−1就可以递推求出任意时刻的状态(也就是任意时刻位移和速度)。
2. 有外部输入的理想的系统模型
2.1 有外部输入理想模型理论定义
考虑外部输入,则线性系统建模成如下形式,系统k时刻的状态
x
k
x_k
xk取决于k-1时刻系统状态
x
k
−
1
x_{k-1}
xk−1和外部输入
u
k
−
1
u_{k-1}
uk−1,且是一个线性关系,其中
u
k
−
1
u_{k-1}
uk−1需要是准确且已知的,数学表达式为:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}
xk=Axk−1+Buk−1
当已知
x
k
−
1
x_{k-1}
xk−1和
u
k
−
1
u_{k-1}
uk−1只能求出当前时刻的状态
x
k
x_k
xk,如果需要继续求后续状态则还需要知道新的外部输入。
2.2 有外部输入理想模型例子分析
现在在k-1到k时刻这个 Δ t \Delta t Δt,人为给物体施加了一个加速度 a a a,也就是给一个恒力。物体不再匀速直线运动。
ps:这个加速度并非是系统状态,而是外部主动施加的,准确已知的。根据位移速度加速度的关系。
数学模型变成了:
x
k
=
[
s
k
v
k
]
=
[
1
Δ
t
0
1
]
[
s
k
−
1
v
k
−
1
]
+
[
1
2
Δ
t
2
0
Δ
t
0
]
[
a
0
]
=
A
x
k
−
1
+
B
u
k
−
1
x_k=\begin{bmatrix} s_k\\v_k\end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\0 & 1 \end{bmatrix}\begin{bmatrix} s_{k-1}\\v_{k-1}\end{bmatrix} + \begin{bmatrix} \frac{1}{2}{\Delta t}^2 & 0\\\Delta t & 0 \end{bmatrix}\begin{bmatrix} a\\0\end{bmatrix} = Ax_{k-1} + Bu_{k-1}
xk=[skvk]=[10Δt1][sk−1vk−1]+[21Δt2Δt00][a0]=Axk−1+Buk−1
也就是:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}
xk=Axk−1+Buk−1
3. 并不理想的系统
3.1 有外部输入非理想模型理论定义
实际上,系统并不完美。
1. 不存在完美模型,这个式子本身可能有问题。
2. 存在不可控的系统扰动,该部分难以被建模。
k时刻的状态
x
k
x_k
xk无法由k-1时刻状态
x
k
−
1
x_{k-1}
xk−1和外部输入
u
k
−
1
u_{k-1}
uk−1准确求解,而是存在不确定性,使用一个随机向量来进行描述,假设这个随机向量对系统状态的影响也是线性的,建模如下:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}
xk=Axk−1+Buk−1+wk−1
3.2 有外部输入非理想模型例子分析
在该例子中,在2的基础上,由于物体存在随机的顺逆风,会对系统进行扰动,
x
k
x_k
xk不再能由
x
k
−
1
x_{k-1}
xk−1,
u
k
−
1
u_{k-1}
uk−1准确求出,而是出现了一个随机偏差。
数学模型变成了:
x
k
=
[
s
k
v
k
]
=
[
1
Δ
t
0
1
]
[
s
k
−
1
v
k
−
1
]
+
[
1
2
Δ
t
2
0
Δ
t
0
]
[
a
0
]
+
[
w
s
k
−
1
w
v
k
−
1
]
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
x_k=\begin{bmatrix} s_k\\v_k\end{bmatrix} = \begin{bmatrix} 1 & \Delta t\\0 & 1 \end{bmatrix}\begin{bmatrix} s_{k-1}\\v_{k-1}\end{bmatrix} + \begin{bmatrix} \frac{1}{2}{\Delta t}^2 & 0\\\Delta t & 0 \end{bmatrix}\begin{bmatrix} a\\0\end{bmatrix} + \begin{bmatrix} w_{s_{k-1}}\\w_{v_{k-1}}\end{bmatrix} = Ax_{k-1} + Bu_{k-1} + w_{k-1}
xk=[skvk]=[10Δt1][sk−1vk−1]+[21Δt2Δt00][a0]+[wsk−1wvk−1]=Axk−1+Buk−1+wk−1
也就是:
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}
xk=Axk−1+Buk−1+wk−1
w
k
−
1
w_{k-1}
wk−1就是由于随机风向引起的偏差,风大一点速度就更快一点,走的更远一点,
x
k
x_k
xk就大一点;风小一点就反之。
4. 由观测状态模型对系统状态进行估计
以下内容的前提是过程噪声 w k − 1 w_{k-1} wk−1满足正态分布 N ( 0 , Q ) N(0,Q) N(0,Q), Q Q Q是观测噪声的协方差矩阵。
同时假设 x ˆ k − 1 \^x_{k-1} xˆk−1是 x k − 1 x_{k-1} xk−1的无偏估计,即 E ( x k − 1 ) = x k − 1 E(x_{k-1})=x_{k-1} E(xk−1)=xk−1
x
k
=
A
x
k
−
1
+
B
u
k
−
1
+
w
k
−
1
x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1}
xk=Axk−1+Buk−1+wk−1
此时想要估计x_k其实就是一个参数估计问题。
由于
E
(
A
x
ˆ
k
−
1
+
B
u
k
−
1
+
w
k
−
1
)
=
x
k
E(A\^x_{k-1}+Bu_{k-1}+w_{k-1})=x_k
E(Axˆk−1+Buk−1+wk−1)=xk,且k时刻可获取的样本数只有
z
k
z_k
zk一个,可以直接用
x
ˆ
k
E
S
T
=
A
x
ˆ
k
−
1
+
B
u
k
−
1
+
w
k
−
1
\^x^{EST}_k=A\^x_{k-1}+Bu_{k-1}+w_{k-1}
xˆkEST=Axˆk−1+Buk−1+wk−1作为
x
k
x_k
xk的矩估计,同时这也是最大似然估计。该估计的协方差先不管他了。晚点再写
观测模型
1. 理想观测模型
1.1 理想观测模型定义
对于线性系统,认为理想传感器观测的数据
z
k
z_k
zk和系统状态
x
k
x_k
xk呈现线性关系。
z
k
=
H
x
k
z_k=Hx_k
zk=Hxk
当H可逆的时候,传感器观测数据可以直接反解出系统状态。
x
k
=
H
−
z
k
x_k = H^-z_k
xk=H−zk
1.2 理想观测模型例子分析
现在你有一个理想gps,可以测到每个时刻物体的位移和速度,并且以数字化的形式给出。
- 测出的读数 z 1 k z_{1_k} z1k和位移的关系 z 1 k = p s z s k z_{1_k} = p_{sz}s_k z1k=pszsk, p s z p_{sz} psz是一个比例系数,表征了实际位移和gps读数 z 1 k z_{1_k} z1k的关系(数字化映射导致有一个缩放关系)。
- 测出的读数 z 2 k z_{2_k} z2k和速度的关系 z 2 k = p v z v k z_{2_k} = p_{vz}v_k z2k=pvzvk, p v z p_{vz} pvz是一个比例系数,表征了实际速度和gps读数 z 2 k z_{2_k} z2k的关系(数字化映射导致有一个缩放关系)。
- 测量值和系统状态的关系
z k = [ z 1 k z 2 k ] = [ p s z 0 0 p v z ] [ s k v k ] = H x k z_k=\begin{bmatrix} z_{1_k}\\z_{2_k}\end{bmatrix} = \begin{bmatrix} p_{sz}& 0\\0 & p_{vz} \end{bmatrix}\begin{bmatrix} s_k\\v_k\end{bmatrix} = Hx_k zk=[z1kz2k]=[psz00pvz][skvk]=Hxk
也就是 z k = H x k z_k=Hx_k zk=Hxk
2. 实际观测模型
2.1 实际观测模型定义
实际上,理想传感器并不存在,
观测数据往往存在噪声,传感数据是
H
x
k
Hx_k
Hxk基础上施加了一个噪声向量
V
k
V_k
Vk(并非是速度,注意区分)
z
k
=
H
x
k
+
V
k
z_k=Hx_k+V_k
zk=Hxk+Vk
2.2 实际观测模型例子分析
现在你的gps是地摊货,读数有很大的测量随机噪声
- 测量值和系统状态的关系变成了
z k = [ z 1 k z 2 k ] = [ p s z 0 0 p v z ] [ s k v k ] + [ V 1 k V 2 k ] = H x k + V k z_k=\begin{bmatrix} z_{1_k}\\z_{2_k}\end{bmatrix} = \begin{bmatrix} p_{sz}& 0\\0 & p_{vz} \end{bmatrix}\begin{bmatrix} s_k\\v_k\end{bmatrix} + \begin{bmatrix} V_{1_k}\\V_{2_k}\end{bmatrix} = Hx_k + V_k zk=[z1kz2k]=[psz00pvz][skvk]+[V1kV2k]=Hxk+Vk
也就是 z k = H x k + V k z_k = Hx_k+V_k zk=Hxk+Vk
3. 由观测状态模型对系统状态进行估计
以下内容的前提是观测噪声满足正态分布 N ( 0 , R ) N(0,R) N(0,R),R是观测噪声的协方差矩阵,且H可逆
由
z
k
=
H
x
k
+
V
k
z_k = Hx_k+V_k
zk=Hxk+Vk可以得出
z
k
∼
N
(
H
x
k
,
R
)
z_k \sim N(Hx_k,R)
zk∼N(Hxk,R),此时想要估计
x
k
x_k
xk其实就是一个参数估计问题。
由于
E
(
z
k
)
=
H
x
k
E(z_k)=Hx_k
E(zk)=Hxk,且k时刻可获取的样本数只有
z
k
z_k
zk一个,可以直接用
x
ˆ
k
M
E
A
=
H
−
z
k
\^x^{MEA}_k=H^-z_k
xˆkMEA=H−zk作为
x
k
x_k
xk的矩估计,同时这也是最大似然估计。
观测状态方程得出的估计量满足
x
ˆ
k
M
E
A
∼
N
(
x
k
,
H
−
R
(
H
−
)
T
)
\^x^{MEA}_k \sim N(x_k,H^-R(H^-)^T)
xˆkMEA∼N(xk,H−R(H−)T)
R H x = H R x H T R_{Hx}=HR_xH^T RHx=HRxHT
推导过程如下
R x = E ( ( x − E x ) ( x − E x ) T ) = E ( x x T ) − E x E x T R_x=E((x-Ex)(x-Ex)^T)=E(xx^T)-Ex{Ex}^T Rx=E((x−Ex)(x−Ex)T)=E(xxT)−ExExT
R H x = E ( ( H x − H E x ) ( H x − H E x ) T ) = E ( H x x T H T ) − H E x E x T H T = H R x H T R_{Hx}=E((Hx-HEx)(Hx-HEx)^T)=E(Hxx^TH^T)-HEx{Ex}^TH^T=HR_xH^T RHx=E((Hx−HEx)(Hx−HEx)T)=E(HxxTHT)−HExExTHT=HRxHT
状态空间方程
包含两个方程
- 系统状态方程
x k = A x k − 1 + B u k − 1 + w k − 1 x_k = Ax_{k-1}+Bu_{k-1}+w_{k-1} xk=Axk−1+Buk−1+wk−1 - 观测状态方程
z k = H x k + V k z_k = Hx_k+V_k zk=Hxk+Vk
x k x_k xk:状态变量
A A A:状态矩阵
B B B:控制矩阵
u k − 1 u_{k-1} uk−1:输入控制矢量
w k − 1 w_{k-1} wk−1:过程噪声,无法建模
V k V_k Vk:测量噪声
H H H:观测矩阵 - 自然界一般假设
V k − 1 ~ N ( 0 , R ) V_{k-1}~N(0,R) Vk−1~N(0,R),是一个随机矢量, R R R是 w w w的协方差矩阵。
w k − 1 ~ N ( 0 , Q ) w_{k-1}~N(0,Q) wk−1~N(0,Q),是一个随机矢量, Q Q Q是 w w w的协方差矩阵。 - 理想情况
在理想模型的情况下,只要有一个公式就可以求解出系统变量 - 实际情况
由于有无法建模的噪声,两个公式求出来的都不太准 - 数据融合
如何用系统方程和观测方程这两个不太准确的方程,最终测出一个相对来说比单个方程测量都准确的数据,并且是通过线性加权进行的数据融合,这就是卡尔曼滤波。