卡尔曼滤波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的协方差矩阵。 - 理想情况
在理想模型的情况下，只要有一个公式就可以求解出系统变量 - 实际情况
由于有无法建模的噪声，两个公式求出来的都不太准 - 数据融合
如何用系统方程和观测方程这两个不太准确的方程，最终测出一个相对来说比单个方程测量都准确的数据，并且是通过线性加权进行的数据融合，这就是卡尔曼滤波。
1072
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
