本讲的核心问题就是线性系统卡尔曼滤波的推导。
离散时间的批量估计
线性高斯模型
运动方程:
x
k
=
A
k
−
1
x
k
−
1
+
v
k
+
w
k
,
k
=
1
,
2
…
…
K
x_k=A_{k-1}x_{k-1}+v_k+w_k,k=1,2……K
xk=Ak−1xk−1+vk+wk,k=1,2……K
观测方程:
y
k
=
C
k
x
k
+
n
k
,
k
=
0
,
1
,
2
…
…
K
y_k=C_kx_k+n_k,k=0,1,2……K
yk=Ckxk+nk,k=0,1,2……K
各个变量的意义:
系统状态:
x
k
∈
R
N
x_k\in\R^N
xk∈RN
初始状态:
x
0
∈
R
N
∼
N
(
x
0
ˇ
,
P
0
ˇ
)
x_0\in\R^N{\sim} N(\check{x_0},\check{P_0})
x0∈RN∼N(x0ˇ,P0ˇ)
输入:
v
k
∈
R
N
v_k\in\R^N
vk∈RN
状态转移矩阵:
A
k
−
1
A_{k-1}
Ak−1
过程噪声:
w
k
∈
R
N
∼
N
(
0
,
Q
k
)
w_k\in\R^N{\sim}N(0,Q_k)
wk∈RN∼N(0,Qk)
观测矩阵:
C
K
C_K
CK
观测噪声:
n
k
∈
R
N
∼
N
(
0
,
R
k
)
n_k\in\R^N{\sim}N(0,R_k)
nk∈RN∼N(0,Rk)
观测量:
y
k
∈
R
N
y_k\in\R^N
yk∈RN
状态估计问题: 通过初始状态、各时刻的观测数据、输入数据,估计系统的真实状态
最大后验估计(MAP)
MAP问题:已知输入和观测,求最大概率的状态
x
^
=
a
r
g
m
a
x
x
p
(
x
∣
y
,
v
)
\hat{x}=arg\underset{x}{max}p(x|y,v)
x^=argxmaxp(x∣y,v)
用贝叶斯公式重写上述公式
x
^
=
a
r
g
m
a
x
x
p
(
y
∣
x
,
v
)
∗
p
(
x
∣
v
)
p
(
y
∣
v
)
=
a
r
g
m
a
x
x
p
(
x
∣
v
)
p
(
y
∣
x
)
\hat{x}=arg\underset{x}{max}\frac{p(y|x,v)*p(x|v)}{p(y|v)}=arg\underset{x}{max}p(x|v)p(y|x)
x^=argxmaxp(y∣v)p(y∣x,v)∗p(x∣v)=argxmaxp(x∣v)p(y∣x)
由于各个时刻观测、输入的噪声都是无关的,上面两个项可以因式分解:
p
(
x
∣
v
)
=
p
(
x
0
∣
x
ˇ
0
)
∏
k
=
1
K
p
(
x
k
,
∣
x
k
−
1
,
v
k
)
p(x|v)=p(x_0|\check x_0)\prod_{k=1}^Kp(x_k,|x_{k-1},v_k)
p(x∣v)=p(x0∣xˇ0)∏k=1Kp(xk,∣xk−1,vk)
p
(
y
∣
x
)
=
∏
k
=
0
K
p
(
y
k
∣
x
k
)
p(y|x)=\prod_{k=0}^Kp(y_k|x_k)
p(y∣x)=∏k=0Kp(yk∣xk)
对目标函数取对数可得:
ln
(
p
(
y
∣
x
)
p
(
x
∣
v
)
)
=
ln
p
(
x
0
∣
x
0
ˇ
)
+
∑
k
=
1
K
ln
p
(
x
k
∣
x
k
−
1
,
v
k
)
+
∑
k
=
0
K
ln
p
(
y
k
∣
x
k
)
\ln(p(y|x)p(x|v))=\ln p(x_0|\check{x_0})+\sum_{k=1}^K \ln p(x_k|x_{k-1},v_k)+\sum_{k=0}^K \ln p(y_k|x_k)
ln(p(y∣x)p(x∣v))=lnp(x0∣x0ˇ)+∑k=1Klnp(xk∣xk−1,vk)+∑k=0Klnp(yk∣xk)
将概率化成相应的数学形式有:
ln
p
(
x
0
∣
x
ˇ
0
)
=
−
1
2
(
x
0
−
x
ˇ
0
)
T
P
ˇ
0
−
1
(
x
0
−
x
ˇ
0
)
−
1
2
ln
(
(
2
π
)
N
d
e
t
P
ˇ
0
)
\ln p(x_0|\check x_0)=- \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0)- \frac{1}{2}\ln((2\pi)^Ndet\check P_0)
lnp(x0∣xˇ0)=−21(x0−xˇ0)TPˇ0−1(x0−xˇ0)−21ln((2π)NdetPˇ0)
ln
p
(
x
k
∣
x
k
−
1
,
v
k
)
=
−
1
2
(
x
k
−
A
k
−
1
x
k
−
1
−
v
k
)
T
Q
k
−
1
(
x
k
−
A
k
−
1
x
k
−
1
−
v
k
)
−
1
2
ln
(
(
2
π
)
N
d
e
t
Q
k
)
\ln p(x_k|x_{k-1},v_k)=-\frac{1}{2}(x_k-A_{k-1}x_{k-1}-v_k)^TQ_k^{-1}(x_k-A_{k-1}x_{k-1}-v_k)-\frac{1}{2}\ln((2\pi)^NdetQ_k)
lnp(xk∣xk−1,vk)=−21(xk−Ak−1xk−1−vk)TQk−1(xk−Ak−1xk−1−vk)−21ln((2π)NdetQk)
ln
p
(
y
k
∣
x
k
)
=
−
1
2
(
y
k
−
C
k
x
k
)
T
R
k
−
1
(
y
k
−
C
k
x
k
)
−
1
2
ln
(
(
2
π
)
M
d
e
t
R
k
)
\ln p(y_k|x_k)=- \frac{1}{2}(y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k)- \frac{1}{2}\ln ((2\pi)^MdetR_k)
lnp(yk∣xk)=−21(yk−Ckxk)TRk−1(yk−Ckxk)−21ln((2π)MdetRk)
去掉与
x
x
x无关的项,定义如下等式:
J
v
,
k
(
x
)
=
{
−
1
2
(
x
0
−
x
ˇ
0
)
T
P
ˇ
0
−
1
(
x
0
−
x
ˇ
0
)
,
k
=
0
−
1
2
(
x
k
−
A
k
−
1
x
k
−
1
−
v
k
)
T
Q
k
−
1
(
x
k
−
A
k
−
1
x
k
−
1
−
v
k
)
,
k
=
1
,
…
…
K
J_{v,k}(x)=\begin{cases} - \frac{1}{2}(x_0-\check x_0)^T\check P_0^{-1}(x_0-\check x_0) ,k=0\\ -\frac{1}{2}(x_k-A_{k-1}x_{k-1}-v_k)^TQ_k^{-1}(x_k-A_{k-1}x_{k-1}-v_k),k=1,……K \end{cases}
Jv,k(x)={−21(x0−xˇ0)TPˇ0−1(x0−xˇ0),k=0−21(xk−Ak−1xk−1−vk)TQk−1(xk−Ak−1xk−1−vk),k=1,……K
J
y
,
k
(
x
)
=
−
1
2
(
y
k
−
C
k
x
k
)
T
R
k
−
1
(
y
k
−
C
k
x
k
)
,
k
=
0
,
…
…
,
K
J_{y,k}(x)=- \frac{1}{2}(y_k-C_kx_k)^TR_k^{-1}(y_k-C_kx_k),k=0,……,K
Jy,k(x)=−21(yk−Ckxk)TRk−1(yk−Ckxk),k=0,……,K
于是目标函数变成最小二乘问题:
x
^
=
arg
m
x
a
x
J
x
,
J
x
=
∑
k
=
0
K
(
J
v
,
k
(
x
)
+
J
y
,
k
(
x
)
)
\hat x=\arg \underset{x}maxJ_x, J_x=\sum_{k=0}^K(J_{v,k}(x)+J_{y,k}(x))
x^=argxmaxJx,Jx=∑k=0K(Jv,k(x)+Jy,k(x))
写成更紧凑的矩阵形式:
把运动和观测写在一起:
z
=
H
x
+
W
z=Hx+W
z=Hx+W
提升形式的目标函数:
J
(
x
)
=
1
2
(
z
−
H
x
)
T
W
−
1
(
z
−
H
x
)
J(x)=\frac {1}{2}(z-Hx)^T W^{-1}(z-Hx)
J(x)=21(z−Hx)TW−1(z−Hx)
该式是个二次的,求其最小值,只令自变量最小值导数为0:
∂
J
(
x
)
∂
x
T
=
−
H
T
W
−
1
(
z
−
H
x
^
)
=
0
\frac {\partial J(x)}{\partial {x^T}}=-H^TW^{-1}(z-H\hat x)=0
∂xT∂J(x)=−HTW−1(z−Hx^)=0
⇒
(
H
T
W
−
1
H
)
x
^
=
H
T
W
−
1
z
\Rightarrow(H^TW^{-1}H)\hat x=H^TW^{-1}z
⇒(HTW−1H)x^=HTW−1z
贝叶斯推断
在LG(线性高斯)系统中,可以根据运动方程和观测方程显式写出状态变量分布的变化过程。
单个时刻:
x
k
=
A
k
−
1
x
k
−
1
+
v
k
+
w
k
x_k=A_{k-1}x_{k-1}+v_k+w_k
xk=Ak−1xk−1+vk+wk
提升形式:
x
=
A
(
v
+
w
)
x=A(v+w)
x=A(v+w)
A
=
[
1
A
0
1
A
1
A
0
A
1
1
⋮
⋮
⋮
A
K
−
1
…
A
0
A
K
−
2
…
A
1
A
K
−
2
…
A
2
…
1
A
K
−
1
…
A
0
A
K
−
1
…
A
1
A
K
−
1
…
A
2
…
A
K
−
1
1
]
A=\begin{bmatrix} 1 \\ A_0 & 1 \\ A_1A_0 & A_1 & 1 \\ \vdots &\vdots &\vdots \\ A_{K-1}…A_0 & A_{K-2}…A_1 & A_{K-2}…A_2 & … & 1 \\ A_{K-1}…A_0 &A_{K-1}…A_1 & A_{K-1}…A_2&…&A_{K-1} & 1\end{bmatrix}
A=⎣⎢⎢⎢⎢⎢⎢⎢⎡1A0A1A0⋮AK−1…A0AK−1…A01A1⋮AK−2…A1AK−1…A11⋮AK−2…A2AK−1…A2……1AK−11⎦⎥⎥⎥⎥⎥⎥⎥⎤
提升形式里,右侧只有v和w,容易求得其均值和协方差:
x
ˇ
=
E
[
x
]
=
E
[
A
(
v
+
w
)
]
=
A
v
\check x=E[x]=E[A(v+w)]=Av
xˇ=E[x]=E[A(v+w)]=Av
P
ˇ
=
E
[
(
x
−
E
[
x
]
)
(
x
−
E
[
x
]
)
T
]
=
A
Q
A
T
\check P=E[(x-E[x])(x-E[x])^T]=AQA^T
Pˇ=E[(x−E[x])(x−E[x])T]=AQAT
先验部分写为:
p
(
x
∣
v
)
=
N
(
x
ˇ
,
P
ˇ
)
=
N
(
A
v
,
A
Q
A
T
)
p(x|v)=N(\check x,\check P)=N(Av,AQA^T)
p(x∣v)=N(xˇ,Pˇ)=N(Av,AQAT)
观测模型:
单次观测:
y
k
=
C
k
x
k
+
n
k
y_k=C_kx_k+n_k
yk=Ckxk+nk
提升形式:
y
=
C
x
+
n
,
C
=
d
i
a
g
(
C
0
,
C
1
,
…
…
C
K
)
y=Cx+n,C=diag(C_0,C_1,……C_K)
y=Cx+n,C=diag(C0,C1,……CK)
联合分布:
p
(
x
,
y
∣
v
)
=
N
(
[
x
ˇ
C
x
ˇ
]
,
[
P
ˇ
P
ˇ
C
T
C
P
ˇ
C
P
ˇ
C
T
+
R
]
)
p(x,y|v)=N(\begin{bmatrix} \check x \\ C\check x\end{bmatrix},\begin{bmatrix} \check P & \check PC^T \\ C\check P & C\check PC^T+R\end{bmatrix})
p(x,y∣v)=N([xˇCxˇ],[PˇCPˇPˇCTCPˇCT+R])
由第一章的高斯推断可得:
p
(
x
∣
v
,
y
)
=
N
(
x
ˇ
+
P
ˇ
C
T
(
C
P
ˇ
C
T
+
R
)
−
1
(
y
−
C
x
ˇ
)
,
P
ˇ
−
P
ˇ
C
T
(
C
P
ˇ
C
T
+
R
)
−
1
C
P
ˇ
)
p(x|v,y)=N(\check x+\check PC^T(C\check PC^T+R)^{-1}(y-C\check x),\check P- \check PC^T(C\check PC^T+R)^{-1}C\check P)
p(x∣v,y)=N(xˇ+PˇCT(CPˇCT+R)−1(y−Cxˇ),Pˇ−PˇCT(CPˇCT+R)−1CPˇ)
带入SMW式进行化简可得:
p
(
x
∣
v
,
y
)
=
N
(
(
P
ˇ
−
1
+
C
T
R
−
1
C
)
−
1
(
P
ˇ
−
1
x
ˇ
+
C
T
R
−
1
y
)
⏟
,
(
P
ˇ
−
1
+
C
T
R
−
1
C
)
−
1
)
⏟
均
值
x
^
后
验
协
方
差
P
^
p(x|v,y)=N(\begin{matrix} (\underbrace{\check P^{-1}+C^TR^{-1}C)^{-1}(\check P^{-1}\check x+C^TR^{-1}y)} , &\underbrace {(\check P^{-1}+C^TR^{-1}C)^{-1})} \\ {均值\hat x} & 后验协方差\hat P\end{matrix}
p(x∣v,y)=N((
Pˇ−1+CTR−1C)−1(Pˇ−1xˇ+CTR−1y),均值x^
(Pˇ−1+CTR−1C)−1)后验协方差P^
均值部分:
(
P
ˇ
−
1
+
C
T
R
−
1
C
)
x
^
=
P
ˇ
−
1
x
ˇ
+
C
T
R
−
1
y
(\check P^{-1}+C^TR^{-1}C)\hat x=\check P^{-1}\check x+C^TR^{-1}y
(Pˇ−1+CTR−1C)x^=Pˇ−1xˇ+CTR−1y
代入
x
ˇ
=
A
v
\check x=Av
xˇ=Av和
P
ˇ
−
1
=
A
−
T
Q
−
1
A
−
1
\check P^{-1}=A^{-T}Q^{-1}A^{-1}
Pˇ−1=A−TQ−1A−1
得均值式:
(
A
−
T
Q
−
1
A
−
1
+
C
T
R
−
1
C
)
x
^
=
A
−
T
Q
−
1
v
+
C
T
R
−
1
y
(A^{-T}Q^{-1}A^{-1}+C^{T}R^{-1}C)\hat x=A^{-T}Q^{-1}v+C^{T}R^{-1}y
(A−TQ−1A−1+CTR−1C)x^=A−TQ−1v+CTR−1y
由于A的结构,A逆具有特殊形式:
A
−
1
=
[
1
−
A
0
1
−
A
1
1
−
A
2
⋱
⋱
1
−
A
K
−
1
1
]
A^{-1}=\begin{bmatrix} 1\\ -A_0 & 1 \\ &-A_1 & 1 \\ & & -A_2 & \ddots \\ & & \ddots & 1 \\ & & & -A_{K-1} & 1\end{bmatrix}
A−1=⎣⎢⎢⎢⎢⎢⎢⎡1−A01−A11−A2⋱⋱1−AK−11⎦⎥⎥⎥⎥⎥⎥⎤
按照均值式,定义矩阵:
z
=
[
v
y
]
,
H
=
[
A
−
1
C
]
,
W
=
[
Q
R
]
z=\begin{bmatrix} v \\ y\end{bmatrix},H=\begin{bmatrix} A^{-1} \\ C\end{bmatrix},W=\begin{bmatrix} Q & \\ &R\end{bmatrix}
z=[vy],H=[A−1C],W=[QR]
可得:
(
H
T
W
−
1
H
)
x
^
=
H
T
W
−
1
z
(H^TW^{-1}H)\hat x=H^TW^{-1}z
(HTW−1H)x^=HTW−1z与MAP结果完全一致!
MAP结果和贝叶斯推断结果一致,说明了什么?
• MAP只关心达到最大后验概率的一个点,这个点的状态称为MAP估计。
• 而贝叶斯推断写出了p(x|y,v)的完整形式,它是一个高斯分布,其均值与MAP估计相等;同时,给出了这个估计的协方差。
• 如果我们只关心状态估计变量取值,那么MAP给出了后验分布的模(Mode),贝叶斯推断给出了均值。
• 而在LG系统中,二者是一样的,使得这两类方法给出了同样的结果。
LG系统最优估计结果唯一的条件:
r
a
n
k
(
H
T
W
−
1
H
)
=
N
(
K
+
1
)
,
N
(
K
+
1
)
表
示
x
的
维
度
rank(H^TW^{-1}H)=N(K+1),N(K+1)表示x的维度
rank(HTW−1H)=N(K+1),N(K+1)表示x的维度。
由于协方差矩阵的对称正定性,即要求
r
a
n
k
(
H
T
H
)
=
r
a
n
k
(
H
T
)
=
N
(
K
+
1
)
rank(H^TH)=rank(H^T)=N(K+1)
rank(HTH)=rank(HT)=N(K+1)
H的具体形式取决于问题有没有0时刻的先验条件。
离散时间的递归平滑算法
很多在线问题当中(比如定位),我们有上一个时刻的先验估计,希望通过这个时刻的控制和观测,计算这个时刻的状态估计。这里介绍递归解法。递归解法的基础是批量问题的解法。
批量问题的核心是用Cholesky分解法求解方程
(
H
T
W
−
1
H
)
x
^
=
H
T
W
−
1
z
(H^TW^{-1}H)\hat x=H^TW^{-1}z
(HTW−1H)x^=HTW−1z
Cholesky解方程的流程:
•Cholesky分解:
H
T
W
−
1
H
=
L
L
T
H^T W^{-1}H=LL^T
HTW−1H=LLT
• 先解:
L
d
=
H
T
W
−
1
z
Ld=H^TW^{-1}z
Ld=HTW−1z 得到d,从上往下解;
• 再解:
L
T
x
^
=
d
L^T\hat x=d
LTx^=d得到最优状态,从下往上解;
• 注意这种解法对一般线性方程也是有效的,不光是针对状态估计问题
• 这两步分别称为前向过程和后向过程(forward/backward)。
迭代法是建立在Cholesky基础上,最终得到经典的RTS Smoother:
前向:
k
=
1
,
…
…
,
K
k=1,……,K
k=1,……,K
P
ˇ
k
,
f
=
A
k
−
1
P
^
k
−
1
,
f
A
k
−
1
T
+
Q
k
\check P_{k,f}=A_{k-1}\hat P_{k-1,f}A_{k-1}^T+Q_k
Pˇk,f=Ak−1P^k−1,fAk−1T+Qk
x
ˇ
k
,
f
=
A
k
−
1
x
^
k
−
1
,
f
+
v
k
\check x_{k,f}=A_{k-1}\hat x_{k-1,f}+v_k
xˇk,f=Ak−1x^k−1,f+vk
K
k
=
P
ˇ
k
,
f
C
k
T
(
C
k
P
ˇ
k
,
f
C
k
T
+
R
k
)
−
1
K_k=\check P_{k,f}C_k^T(C_k\check P_{k,f}C_k^T+R_k)^{-1}
Kk=Pˇk,fCkT(CkPˇk,fCkT+Rk)−1
P
^
k
,
f
=
(
1
−
K
k
C
k
)
P
ˇ
k
,
f
\hat P_{k,f}=(1-K_kC_k)\check P_{k,f}
P^k,f=(1−KkCk)Pˇk,f
x
^
k
,
f
=
x
ˇ
k
,
f
+
K
k
(
y
k
−
C
k
x
ˇ
k
,
f
)
\hat x_{k,f}=\check x_{k,f}+K_k(y_k-C_k\check x_{k,f})
x^k,f=xˇk,f+Kk(yk−Ckxˇk,f)
后向:
k
=
K
,
…
…
,
1
k=K,……,1
k=K,……,1
x
^
k
−
1
=
x
^
k
−
1
,
f
+
P
^
k
−
1
,
f
A
k
−
1
T
P
ˇ
k
,
f
−
1
(
x
^
k
−
x
ˇ
k
,
f
)
\hat x_{k-1}=\hat x_{k-1,f}+\hat P_{k-1,f}A_{k-1}^T\check P_{k,f}^{-1}(\hat x_k-\check x_{k,f})
x^k−1=x^k−1,f+P^k−1,fAk−1TPˇk,f−1(x^k−xˇk,f)
离散时间的滤波算法
• RTS Smoother是无法在线运行的(非因果的Not causal)
• 它的后向迭代过程使用下个时刻的信息更新之前的估计
• 初始值中需要知道x(K)的后验
利用MAP和贝叶斯推断均可推导出卡尔曼滤波,在此只给出卡尔曼滤波最终形式:
关于卡尔曼滤波器的结论
• 卡尔曼滤波器给出了LG系统下最优线性无偏估计(Best Linear Unbiased Estimate, BLUE)
• 需要有初始状态
• 卡尔曼滤波器即RTS Smoother的前向部分
• 在非线性场合下,我们会使用扩展卡尔曼滤波器(EKF),但此时MAP、贝叶斯推断、EKF给出的结果均会不一样