平方根滤波
针对经典Kalman滤波中,状态误差阵 P k P_k Pk是状态估计的平方,可能占有更多位数,所以使用状态误差阵 P k P_k Pk的平方根进行更新,减少数值位数和计算误差(在早期计算机中比较有校)。
Potter平方根滤波
将均方误差阵
P
k
P_k
Pk分解为下三角阵,再求平方根
Cholesky分解
P
:
n
阶
正
定
对
称
矩
阵
P:n阶正定对称矩阵
P:n阶正定对称矩阵
总可以得到:
P
=
[
P
11
P
12
.
.
.
P
1
n
P
21
P
22
.
.
.
P
2
n
.
.
.
.
.
.
.
.
.
.
.
.
P
n
1
P
n
2
.
.
.
P
n
n
]
,
Δ
=
[
δ
11
δ
12
.
.
.
δ
1
n
0
δ
21
.
.
.
δ
2
n
.
.
.
.
.
.
.
.
.
.
.
.
0
0
.
.
.
δ
n
n
]
P=\left[\begin{matrix} P_{11}&P_{12}&...&P_{1n}\\P_{21}&P_{22}&...&P_{2n}\\ ...&...&...&...\\P_{n1}&P_{n2}&...&P_{nn} \end{matrix}\right],\Delta=\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right]
P=⎣⎢⎢⎡P11P21...Pn1P12P22...Pn2............P1nP2n...Pnn⎦⎥⎥⎤,Δ=⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤
P
=
Δ
Δ
T
=
[
δ
11
δ
12
.
.
.
δ
1
n
0
δ
21
.
.
.
δ
2
n
.
.
.
.
.
.
.
.
.
.
.
.
0
0
.
.
.
δ
n
n
]
[
δ
11
δ
12
.
.
.
δ
1
n
0
δ
21
.
.
.
δ
2
n
.
.
.
.
.
.
.
.
.
.
.
.
0
0
.
.
.
δ
n
n
]
T
P=\Delta \Delta^T =\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right]\left[\begin{matrix} \delta_{11}&\delta_{12}&...&\delta_{1n}\\0&\delta_{21}&...&\delta_{2n}\\ ...&...&...&...\\0&0&...&\delta_{nn} \end{matrix}\right]^T
P=ΔΔT=⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤⎣⎢⎢⎡δ110...0δ12δ21...0............δ1nδ2n...δnn⎦⎥⎥⎤T
可以得到:
P
i
j
=
δ
i
j
δ
j
j
+
δ
i
,
j
+
1
δ
i
,
j
+
1
+
δ
i
,
j
+
2
δ
i
,
j
+
2
+
.
.
.
+
δ
i
,
j
+
n
δ
i
,
j
+
n
=
∑
k
=
j
+
1
n
δ
i
k
δ
j
k
+
δ
i
j
δ
j
j
(
1
≤
i
≤
n
,
i
≤
j
≤
n
)
P_{ij}=\delta_{ij}\delta_{jj}+\delta_{i,j+1}\delta_{i,j+1}+\delta_{i,j+2}\delta_{i,j+2}+...+\delta_{i,j+n}\delta_{i,j+n}=\sum_{k=j+1}^n\delta_{ik}\delta_{jk}+\delta_{ij}\delta_{jj}(1\leq i\leq n,i\leq j \leq n)
Pij=δijδjj+δi,j+1δi,j+1+δi,j+2δi,j+2+...+δi,j+nδi,j+n=k=j+1∑nδikδjk+δijδjj(1≤i≤n,i≤j≤n)
从而得到:
δ
i
j
=
{
(
P
i
j
−
∑
i
+
j
n
δ
i
k
δ
j
k
)
/
δ
j
j
i
<
j
P
j
j
−
∑
k
=
j
+
1
n
δ
j
k
2
i
=
j
0
i
>
j
\delta_{ij}=\begin{cases} (P_{ij}-\sum_{i+j}^{n}\delta_{ik}\delta_{jk})/\delta_{jj} &i<j\\ \sqrt{P_{jj}-\sum_{k=j+1}^{n}\delta_{jk}^2}&i=j\\ 0&i>j\\ \end{cases}
δij=⎩⎪⎪⎨⎪⎪⎧(Pij−∑i+jnδikδjk)/δjjPjj−∑k=j+1nδjk20i<ji=ji>j
QR分解
A
m
×
n
:
为
列
满
秩
矩
阵
(
m
≥
n
)
,
r
a
n
k
(
A
m
×
n
)
=
n
A_{m×n}:为列满秩矩阵(m\geq n),rank(A_{m×n})=n
Am×n:为列满秩矩阵(m≥n),rank(Am×n)=n
那末必有:
A
m
×
n
=
Q
m
×
n
R
n
×
n
,
Q
m
×
n
T
Q
m
×
n
=
I
n
×
n
,
R
n
×
n
为
上
三
角
阵
或
下
三
角
阵
A_{m×n}=Q_{m×n}R_{n×n},Q_{m×n}^TQ_{m×n}=I_{n×n},R_{n×n}为上三角阵或下三角阵
Am×n=Qm×nRn×n,Qm×nTQm×n=In×n,Rn×n为上三角阵或下三角阵
均方误差的量测更新
系统状态空间
X
k
:
n
维
状
态
向
量
X_k:n维状态向量
Xk:n维状态向量
Z
k
:
m
维
测
量
向
量
Z_k:m维测量向量
Zk:m维测量向量
Φ
k
/
k
−
1
:
已
知
的
系
统
结
构
参
数
\Phi_{k/k-1}:已知的系统结构参数
Φk/k−1:已知的系统结构参数
Γ
k
/
k
−
1
:
已
知
的
系
统
结
构
参
数
,
分
别
为
n
×
l
阶
系
统
分
配
噪
声
\Gamma_{k/k-1}:已知的系统结构参数,分别为n×l阶系统分配噪声
Γk/k−1:已知的系统结构参数,分别为n×l阶系统分配噪声
H
k
:
已
知
的
系
统
结
构
参
数
,
分
别
为
m
×
n
阶
测
量
矩
阵
H_k:已知的系统结构参数,分别为m×n阶测量矩阵
Hk:已知的系统结构参数,分别为m×n阶测量矩阵
V
k
:
m
维
测
量
噪
声
,
高
斯
白
噪
声
,
服
从
正
太
分
布
V_k:m维测量噪声,高斯白噪声,服从正太分布
Vk:m维测量噪声,高斯白噪声,服从正太分布
W
k
−
1
:
m
维
系
统
噪
声
向
量
,
高
斯
白
噪
声
,
服
从
正
太
分
布
W_{k-1}:m维系统噪声向量,高斯白噪声,服从正太分布
Wk−1:m维系统噪声向量,高斯白噪声,服从正太分布
V
k
与
W
k
−
1
互
不
相
关
V_k与W_{k-1}互不相关
Vk与Wk−1互不相关
{
X
k
=
Φ
k
/
k
−
1
X
k
−
1
+
Γ
k
/
k
−
1
W
k
−
1
Z
k
=
H
k
X
k
+
V
k
s
t
.
{
E
[
W
k
]
=
0
,
E
[
W
k
W
j
T
]
=
Q
k
δ
k
j
Q
k
≥
0
E
[
V
k
]
=
0
,
E
[
V
k
V
j
T
]
=
R
k
δ
k
j
,
E
[
W
k
V
j
T
]
=
0
R
≥
0
\begin{cases} X_k=\Phi_{k/k-1}X_{k-1}+\Gamma_{k/k-1}W_{k-1}\\ Z_k=H_kX_k+V_k\\ \end{cases} \\ st. \\ \begin{cases} E[W_k]=0,E[W_kW_j^T]=Q_k\delta_{kj} &Q_k \geq 0\\ E[V_k]=0,E[V_kV_j^T]=R_k\delta_{kj},E[W_kV_j^T]=0&R\geq 0\\ \end{cases}
{Xk=Φk/k−1Xk−1+Γk/k−1Wk−1Zk=HkXk+Vkst.{E[Wk]=0,E[WkWjT]=QkδkjE[Vk]=0,E[VkVjT]=Rkδkj,E[WkVjT]=0Qk≥0R≥0
kalman滤波
{
X
^
k
/
k
−
1
=
Φ
k
/
k
−
1
X
^
k
−
1
状
态
一
步
预
测
P
k
/
k
−
1
=
Φ
k
/
k
−
1
P
k
−
1
Φ
k
/
k
−
1
T
+
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
状
态
一
步
预
测
均
方
差
阵
K
k
=
P
k
/
k
−
1
H
k
T
(
H
k
P
k
/
k
−
1
H
k
T
−
R
k
)
−
1
滤
波
增
益
X
^
k
=
(
I
−
K
k
H
k
)
X
^
k
/
k
−
1
+
K
k
Z
k
状
态
估
计
P
k
=
(
I
−
K
k
H
k
)
P
k
/
k
−
1
状
态
估
计
均
方
误
差
阵
\begin{cases} \hat X_{k/k-1}=\Phi_{k/k-1}\hat X_{k-1}&状态一步预测\\ P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T&状态一步预测均方差阵\\ K_k=P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}&滤波增益\\ \hat X_k=(I-K_kH_k)\hat X_{k/k-1}+K_kZ_k&状态估计\\ P_k=(I-K_kH_k)P_{k/k-1}&状态估计均方误差阵\\ \end{cases}
⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧X^k/k−1=Φk/k−1X^k−1Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1TKk=Pk/k−1HkT(HkPk/k−1HkT−Rk)−1X^k=(I−KkHk)X^k/k−1+KkZkPk=(I−KkHk)Pk/k−1状态一步预测状态一步预测均方差阵滤波增益状态估计状态估计均方误差阵
滤波增益带入状态估计均方误差阵
P
k
=
(
I
−
P
k
/
k
−
1
H
k
T
(
H
k
P
k
/
k
−
1
H
k
T
−
R
k
)
−
1
H
k
)
P
k
/
k
−
1
P_k=(I-P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T-R_k)^{-1}H_k)P_{k/k-1}
Pk=(I−Pk/k−1HkT(HkPk/k−1HkT−Rk)−1Hk)Pk/k−1
假设
P
k
−
1
,
P
k
/
k
−
1
,
P
k
P_{k-1},P_{k/k-1},P_k
Pk−1,Pk/k−1,Pk的平方根分别为
Δ
k
−
1
,
Δ
k
/
k
−
1
,
Δ
k
\Delta_{k-1},\Delta_{k/k-1},\Delta_k
Δk−1,Δk/k−1,Δk
P
k
−
1
=
Δ
k
−
1
Δ
k
−
1
T
,
P
k
/
k
−
1
=
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
,
P
k
=
Δ
k
Δ
k
T
P_{k-1}=\Delta_{k-1}\Delta_{k-1}^T,P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T,P_{k}=\Delta_{k}\Delta_{k}^T
Pk−1=Δk−1Δk−1T,Pk/k−1=Δk/k−1Δk/k−1T,Pk=ΔkΔkT
带入
P
k
P_k
Pk
Δ
k
Δ
k
T
=
P
k
=
Δ
k
/
k
−
1
[
I
−
Δ
k
/
k
−
1
T
H
k
T
(
H
k
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
H
k
T
+
R
k
)
−
1
H
k
Δ
k
/
k
−
1
]
Δ
k
/
k
−
1
T
=
Δ
k
/
k
−
1
(
I
−
ρ
k
−
2
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
Δ
k
/
k
−
1
T
s
t
:
ρ
k
2
=
H
k
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
H
k
T
+
R
k
\Delta_{k}\Delta_{k}^T=P_k=\Delta_{k/k-1}[I-\Delta_{k/k-1}^TH_k^T(H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k)^{-1}H_k\Delta_{k/k-1}]\Delta_{k/k-1}^T \\ =\Delta_{k/k-1}(I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})\Delta_{k/k-1}^T \\ st:\\ \rho_k^2=H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k
ΔkΔkT=Pk=Δk/k−1[I−Δk/k−1THkT(HkΔk/k−1Δk/k−1THkT+Rk)−1HkΔk/k−1]Δk/k−1T=Δk/k−1(I−ρk−2Δk/k−1THkTHkΔk/k−1)Δk/k−1Tst:ρk2=HkΔk/k−1Δk/k−1THkT+Rk
求:
P
k
P_k
Pk的根等价于
I
−
ρ
k
−
2
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}
I−ρk−2Δk/k−1THkTHkΔk/k−1的根
I
−
ρ
k
−
2
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
=
(
I
−
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
(
I
+
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}=(I-r_k^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})(I+r_k^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})
I−ρk−2Δk/k−1THkTHkΔk/k−1=(I−rk−1Δk/k−1THkTHkΔk/k−1)(I+rk−1Δk/k−1THkTHkΔk/k−1)
利用待定系数法求得:
r
k
=
ρ
k
(
ρ
k
±
R
k
)
=
2
ρ
k
2
±
4
ρ
k
4
−
4
ρ
k
2
(
ρ
k
2
−
R
k
)
2
r_k=\rho_k(\rho_k \pm\sqrt{R_k})=\frac{2\rho_k^2\pm\sqrt{4\rho_k^4-4\rho_k^2(\rho_k^2-R_k)}}{2}
rk=ρk(ρk±Rk)=22ρk2±4ρk4−4ρk2(ρk2−Rk)
Δ
k
Δ
k
T
=
Δ
k
/
k
−
1
(
I
−
ρ
k
−
2
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
Δ
k
/
k
−
1
T
=
(
Δ
k
/
k
−
1
(
I
−
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
)
(
Δ
k
/
k
−
1
(
I
−
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
)
T
\Delta_{k}\Delta_{k}^T=\Delta_{k/k-1}(I-\rho_{k}^{-2}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})\Delta_{k/k-1}^T\\ =(\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}))(\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1}))^T
ΔkΔkT=Δk/k−1(I−ρk−2Δk/k−1THkTHkΔk/k−1)Δk/k−1T=(Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1))(Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1))T
Δ
k
=
Δ
k
/
k
−
1
(
I
−
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
(
Δ
k
计
算
之
后
一
般
不
为
三
角
阵
)
\Delta _k=\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})(\Delta _k 计算之后一般不为三角阵)
Δk=Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1)(Δk计算之后一般不为三角阵)
均方误差的时间更新
P
k
−
1
=
Δ
k
−
1
Δ
k
−
1
T
,
P
k
/
k
−
1
=
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
,
P
k
=
Δ
k
Δ
k
T
P_{k-1}=\Delta_{k-1}\Delta_{k-1}^T,P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T,P_{k}=\Delta_{k}\Delta_{k}^T
Pk−1=Δk−1Δk−1T,Pk/k−1=Δk/k−1Δk/k−1T,Pk=ΔkΔkT
带入
P
k
/
k
−
1
=
Φ
k
/
k
−
1
P
k
−
1
Φ
k
/
k
−
1
T
+
Γ
k
−
1
Q
k
−
1
Γ
k
−
1
T
P_{k/k-1}=\Phi_{k/k-1}P_{k-1}\Phi^T_{k/k-1}+\Gamma_{k-1}Q_{k-1}\Gamma_{k-1}^T
Pk/k−1=Φk/k−1Pk−1Φk/k−1T+Γk−1Qk−1Γk−1T
Q
k
−
1
1
/
2
(
Q
k
−
1
1
/
2
)
T
=
Q
k
−
1
(
c
h
o
l
e
s
k
y
分
解
法
求
得
)
Q_{k-1}^{1/2}(Q_{k-1}^{1/2})^T=Q_{k-1}(cholesky 分解法求得)
Qk−11/2(Qk−11/2)T=Qk−1(cholesky分解法求得)
P
k
/
k
−
1
=
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
=
Φ
k
/
k
−
1
Δ
k
−
1
Δ
k
−
1
T
Φ
k
/
k
−
1
T
+
Γ
k
−
1
Q
k
−
1
1
/
2
(
Q
k
−
1
1
/
2
)
T
Γ
k
−
1
T
=
[
Φ
k
/
k
−
1
Δ
k
−
1
Γ
k
−
1
Q
k
−
1
1
/
2
]
[
Δ
k
−
1
T
Φ
k
/
k
−
1
T
(
Q
k
−
1
1
/
2
)
T
Γ
k
−
1
T
]
P_{k/k-1}=\Delta_{k/k-1}\Delta_{k/k-1}^T=\Phi_{k/k-1}\Delta_{k-1}\Delta_{k-1}^T\Phi^T_{k/k-1}+\Gamma_{k-1}Q^{1/2}_{k-1}(Q^{1/2}_{k-1})^T\Gamma_{k-1}^T \\ =\left[\begin{matrix} \Phi_{k/k-1}\Delta_{k-1}&\Gamma_{k-1}Q_{k-1}^{1/2}\\ \end{matrix}\right] \left[\begin{matrix} \Delta_{k-1}^T\Phi_{k/k-1}^T\\(Q_{k-1}^{1/2})^T\Gamma_{k-1}^T\\ \end{matrix}\right]
Pk/k−1=Δk/k−1Δk/k−1T=Φk/k−1Δk−1Δk−1TΦk/k−1T+Γk−1Qk−11/2(Qk−11/2)TΓk−1T=[Φk/k−1Δk−1Γk−1Qk−11/2][Δk−1TΦk/k−1T(Qk−11/2)TΓk−1T]
令
A
(
n
+
l
)
×
n
=
[
Δ
k
−
1
T
Φ
k
/
k
−
1
T
(
Q
k
−
1
1
/
2
)
T
Γ
k
−
1
T
]
A_{(n+l)×n}=\left[\begin{matrix} \Delta_{k-1}^T\Phi_{k/k-1}^T\\(Q_{k-1}^{1/2})^T\Gamma_{k-1}^T\\ \end{matrix}\right]
A(n+l)×n=[Δk−1TΦk/k−1T(Qk−11/2)TΓk−1T],对
A
(
n
+
l
)
×
n
A_{(n+l)×n}
A(n+l)×n进行UD分解
A
(
n
+
l
)
×
n
=
Q
‾
(
n
+
l
)
×
n
R
‾
n
×
n
A_{(n+l)×n}=\overline Q_{(n+l)×n}\overline R_{n×n}
A(n+l)×n=Q(n+l)×nRn×n
A
(
n
+
l
)
×
n
T
A
(
n
+
l
)
×
n
=
(
Q
‾
(
n
+
l
)
×
n
R
‾
n
×
n
)
T
(
Q
‾
(
n
+
l
)
×
n
R
‾
n
×
n
)
=
R
n
×
n
T
R
n
×
n
=
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
A_{(n+l)×n}^TA_{(n+l)×n}=(\overline Q_{(n+l)×n}\overline R_{n×n})^T(\overline Q_{(n+l)×n}\overline R_{n×n})=R_{n×n}^TR_{n×n}=\Delta_{k/k-1}\Delta_{k/k-1}^T
A(n+l)×nTA(n+l)×n=(Q(n+l)×nRn×n)T(Q(n+l)×nRn×n)=Rn×nTRn×n=Δk/k−1Δk/k−1T
Δ
k
/
k
−
1
T
\Delta_{k/k-1}^T
Δk/k−1T为
A
(
n
+
l
)
×
n
A_{(n+l)×n}
A(n+l)×n进行UD分解之后的,上三角或下三角阵
平方根滤波流程
{
a
k
=
Δ
k
/
k
−
1
T
H
k
T
ρ
k
2
=
H
k
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
H
k
T
+
R
k
=
a
k
T
a
k
+
R
k
Δ
k
=
Δ
k
/
k
−
1
(
I
−
r
k
−
1
Δ
k
/
k
−
1
T
H
k
T
H
k
Δ
k
/
k
−
1
)
=
Δ
k
/
k
−
1
(
I
−
r
k
−
1
a
k
a
k
T
)
K
k
=
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
H
k
T
(
H
k
Δ
k
/
k
−
1
Δ
k
/
k
−
1
T
H
k
T
+
R
k
)
−
1
=
Δ
k
/
k
−
1
a
k
ρ
k
−
2
(
或
Δ
k
Δ
k
T
H
k
T
R
k
−
1
)
\begin{cases} a_k=\Delta_{k/k-1}^TH_k^T&\\ \rho_k^2=H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k=a_k^Ta_k+R_k\\ \Delta _k=\Delta_{k/k-1}(I-r_{k}^{-1}\Delta_{k/k-1}^TH_k^TH_k\Delta_{k/k-1})=\Delta_{k/k-1}(I-r^{-1}_ka_ka_k^T)\\ K_k=\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T(H_k\Delta_{k/k-1}\Delta_{k/k-1}^TH_k^T+R_k)^{-1}=\Delta_{k/k-1}a_k\rho_k^{-2}(或\Delta_{k}\Delta_{k}^TH_k^TR_k^{-1})\\ \end{cases}
⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧ak=Δk/k−1THkTρk2=HkΔk/k−1Δk/k−1THkT+Rk=akTak+RkΔk=Δk/k−1(I−rk−1Δk/k−1THkTHkΔk/k−1)=Δk/k−1(I−rk−1akakT)Kk=Δk/k−1Δk/k−1THkT(HkΔk/k−1Δk/k−1THkT+Rk)−1=Δk/k−1akρk−2(或ΔkΔkTHkTRk−1)
当量测为矢量时
上述情况针对量测为标量时的情况,当量测为矢量时:
- 使用序贯滤波将矢量转化为标量
- 直接进行向量量测平方根滤波
此时:均方误差阵及其对应平方根滤波公式为:
P k = P k / k − 1 − P k / k − 1 H k T ( H k P k / k − 1 H k T + R k ) − 1 H k P k / k − 1 Δ k = Δ k / k − 1 [ I − Δ k / k − 1 T H k T ( p k p k T + R k 1 / 2 P k T ) − 1 H k Δ k / k − 1 ] s t : p k p k T = H k P k / k − 1 H k T + R k = [ H k Δ k / k − 1 R k 1 / 2 ] [ Δ k / k − 1 T H k T ( R k 1 / 2 ) T ] − > P k T ( Q R 同 上 Q R 分 解 得 到 ) P_k=P_{k/k-1}-P_{k/k-1}H_k^T(H_kP_{k/k-1}H_k^T+R_k)^{-1}H_kP_{k/k-1} \\ \Delta_k=\Delta_{k/k-1}[I-\Delta_{k/k-1}^TH_k^T(p_kp_k^T+R_k^{1/2}P_k^T)^{-1}H_k\Delta_{k/k-1}]\\ st:\\ p_kp_k^T=H_kP_{k/k-1}H_k^T+R_k=\left[\begin{matrix} H_{k}\Delta_{k/k-1}&R_{k}^{1/2}\\ \end{matrix}\right] \left[\begin{matrix} \Delta_{k/k-1}^TH_{k}^T\\ (R_{k}^{1/2})^T\\ \end{matrix}\right]->P_k^T(QR同上QR分解得到) Pk=Pk/k−1−Pk/k−1HkT(HkPk/k−1HkT+Rk)−1HkPk/k−1Δk=Δk/k−1[I−Δk/k−1THkT(pkpkT+Rk1/2PkT)−1HkΔk/k−1]st:pkpkT=HkPk/k−1HkT+Rk=[HkΔk/k−1Rk1/2][Δk/k−1THkT(Rk1/2)T]−>PkT(QR同上QR分解得到)