采用递推方式求解最小二乘法(Recursive Least Square RLS)可以有效减小数据存储量。
测量方程
Z
‾
k
=
H
‾
k
X
+
V
‾
k
\overline Z_k=\overline H_kX+\overline V_k
Zk=HkX+Vk
Z
‾
k
=
[
Z
1
Z
2
Z
3
.
.
.
Z
i
]
,
H
‾
k
=
[
H
1
H
2
H
3
.
.
.
H
i
]
,
V
‾
k
=
[
V
1
V
2
V
3
.
.
.
V
i
]
E
[
V
‾
i
]
=
0
,
E
[
V
‾
i
V
‾
i
T
]
=
R
‾
i
=
d
i
a
g
(
R
1
,
R
2
,
R
3
,
.
.
.
)
\overline Z_k=\left[\begin{matrix}Z_1\\Z_2\\Z_3\\...\\Z_i \end{matrix}\right],\overline H_k=\left[\begin{matrix}H_1\\H_2\\H_3\\...\\H_i \end{matrix}\right],\overline V_k=\left[\begin{matrix}V_1\\V_2\\V_3\\...\\V_i \end{matrix}\right] \\ E[\overline V_i]=0,E[\overline V_i\overline V_i^T]=\overline R_i=diag(R_1,R_2,R_3,...)
Zk=⎣⎢⎢⎢⎢⎡Z1Z2Z3...Zi⎦⎥⎥⎥⎥⎤,Hk=⎣⎢⎢⎢⎢⎡H1H2H3...Hi⎦⎥⎥⎥⎥⎤,Vk=⎣⎢⎢⎢⎢⎡V1V2V3...Vi⎦⎥⎥⎥⎥⎤E[Vi]=0,E[ViViT]=Ri=diag(R1,R2,R3,...)
最小二乘估计
X
^
W
L
S
=
(
H
T
W
H
)
−
1
H
T
W
Z
\hat X_{WLS}=(H^TWH)^{-1}H^TWZ
X^WLS=(HTWH)−1HTWZ
对测量方程做最小二乘估计
k
−
1
k-1
k−1时最小二乘估计
X
^
k
−
1
=
(
H
‾
k
−
1
T
R
‾
k
−
1
−
1
H
‾
k
−
1
)
−
1
H
‾
k
−
1
T
R
‾
k
−
1
−
1
Z
‾
k
−
1
=
P
k
−
1
H
‾
k
−
1
T
R
‾
k
−
1
−
1
Z
‾
k
−
1
P
k
−
1
=
(
H
‾
k
−
1
T
R
‾
k
−
1
−
1
H
‾
k
−
1
)
−
1
\hat X_{k-1}=(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline H_{k-1})^{-1}\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1}=P_{k-1} \overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1} \\ P_{k-1} =(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline H_{k-1})^{-1}
X^k−1=(Hk−1TRk−1−1Hk−1)−1Hk−1TRk−1−1Zk−1=Pk−1Hk−1TRk−1−1Zk−1Pk−1=(Hk−1TRk−1−1Hk−1)−1
k
k
k时最小二乘估计
X
^
k
=
(
H
‾
k
T
R
‾
k
−
1
H
‾
k
)
−
1
H
‾
k
T
R
‾
k
−
1
Z
‾
k
=
P
k
(
[
H
‾
k
−
1
T
H
k
T
]
[
R
‾
k
−
1
−
1
0
0
R
k
−
1
]
[
Z
‾
k
−
1
Z
k
]
)
=
P
k
(
H
‾
k
−
1
T
R
‾
k
−
1
−
1
Z
‾
k
−
1
+
H
k
T
R
k
−
1
Z
k
)
=
X
^
k
−
1
+
P
k
H
k
T
R
k
−
1
(
Z
k
−
H
k
X
^
k
−
1
)
=
X
^
k
−
1
+
K
k
(
Z
k
−
H
k
X
^
k
−
1
)
\hat X_{k}=(\overline H_{k}^T\overline R^{-1}_{k}\overline H_{k})^{-1}\overline H_{k}^T\overline R^{-1}_{k}\overline Z_{k}\\ =P_k(\left[\begin{matrix} \overline H_{k-1}^T&H_k^T\\ \end{matrix}\right]\left[\begin{matrix} \overline R_{k-1}^{-1}&0\\0&R_k^{-1}\\ \end{matrix}\right]\left[\begin{matrix} \overline Z_{k-1}\\Z_k\\ \end{matrix}\right])\\ =P_k(\overline H_{k-1}^T\overline R^{-1}_{k-1}\overline Z_{k-1} +H_k^TR_k^{-1}Z_k)\\ =\hat X_{k-1}+P_{k}H_{k}^TR_k^{-1}(Z_k-H_k\hat X_{k-1}) \\ =\hat X_{k-1}+K_k(Z_k-H_k\hat X_{k-1})
X^k=(HkTRk−1Hk)−1HkTRk−1Zk=Pk([Hk−1THkT][Rk−1−100Rk−1][Zk−1Zk])=Pk(Hk−1TRk−1−1Zk−1+HkTRk−1Zk)=X^k−1+PkHkTRk−1(Zk−HkX^k−1)=X^k−1+Kk(Zk−HkX^k−1)
P
k
=
H
‾
k
T
R
‾
k
−
1
H
‾
k
=
(
[
H
‾
k
−
1
T
H
k
T
]
[
R
‾
k
−
1
−
1
0
0
R
k
−
1
]
[
Z
‾
k
−
1
Z
k
]
)
−
1
=
(
P
k
−
1
−
1
+
H
k
T
R
k
−
1
H
k
)
−
1
P_k=\overline H_{k}^T\overline R^{-1}_{k}\overline H_{k}=(\left[\begin{matrix} \overline H_{k-1}^T&H_k^T\\ \end{matrix}\right]\left[\begin{matrix} \overline R_{k-1}^{-1}&0\\0&R_k^{-1}\\ \end{matrix}\right]\left[\begin{matrix} \overline Z_{k-1}\\Z_k\\ \end{matrix}\right])^{-1}=(P_{k-1}^{-1}+H_k^TR_k^{-1}H_k)^{-1}
Pk=HkTRk−1Hk=([Hk−1THkT][Rk−1−100Rk−1][Zk−1Zk])−1=(Pk−1−1+HkTRk−1Hk)−1
K
k
=
P
k
H
k
T
R
k
−
1
K_k=P_{k}H_{k}^TR_k^{-1}
Kk=PkHkTRk−1
最小二乘估计递推公式
P
k
−
1
P_{k}^{-1}
Pk−1左乘
P
k
P_k
Pk右乘
P
k
−
1
P_{k-1}
Pk−1
P
k
P
k
−
1
P
k
−
1
=
P
k
(
P
k
−
1
−
1
+
H
k
T
R
k
−
1
H
k
)
P
k
−
1
P_kP_k^{-1}P_{k-1}=P_k(P_{k-1}^{-1}+H_k^TR_k^{-1}H_k)P_{k-1}
PkPk−1Pk−1=Pk(Pk−1−1+HkTRk−1Hk)Pk−1
得到
P
k
P_k
Pk
P
k
=
P
k
−
1
−
P
k
H
k
T
R
k
−
1
H
k
P
k
−
1
P_k=P_{k-1}^{}-P_kH_k^TR_k^{-1}H_kP_{k-1}
Pk=Pk−1−PkHkTRk−1HkPk−1
再将
P
k
P_k
Pk右乘
H
k
T
H_k^T
HkT整理可得:
K
k
=
P
k
H
k
T
R
k
−
1
=
P
k
−
1
H
k
T
(
H
k
P
k
−
1
H
k
T
+
R
k
)
−
1
K_k=P_kH_k^TR_k^{-1}=P_{k-1}H_k^T(H_kP_{k-1}H_k^T+R_k)^{-1}
Kk=PkHkTRk−1=Pk−1HkT(HkPk−1HkT+Rk)−1
得到递推公式为:
{
K
k
=
P
k
−
1
H
k
T
(
H
k
P
k
−
1
H
k
T
+
R
k
)
−
1
P
k
=
P
k
−
1
−
P
k
H
k
T
R
k
−
1
H
k
P
k
−
1
X
^
k
=
X
^
k
−
1
+
K
k
(
Z
k
−
H
k
X
^
k
−
1
)
\begin{cases} K_k=P_{k-1}H_k^T(H_kP_{k-1}H_k^T+R_k)^{-1}\\ P_k=P_{k-1}^{}-P_kH_k^TR_k^{-1}H_kP_{k-1}\\ \hat X_{k}=\hat X_{k-1}+K_k(Z_k-H_k\hat X_{k-1})\\ \end{cases}
⎩⎪⎨⎪⎧Kk=Pk−1HkT(HkPk−1HkT+Rk)−1Pk=Pk−1−PkHkTRk−1HkPk−1X^k=X^k−1+Kk(Zk−HkX^k−1)