32、基于UDUT分解的卡尔曼滤波器及MATLAB实现

基于UDUT分解的卡尔曼滤波器及MATLAB实现

1. UDUT分解下的卡尔曼滤波器解

在卡尔曼滤波器中使用UDUT分解时，初始条件 $P(0)^+$ 的分解结果如下：
$U(0)^+ =
\begin{bmatrix}
1 & 0 & 0 \
0 & 1 & 0 \
0 & 0 & 1
\end{bmatrix}$；
$D(0)^+ =
\begin{bmatrix}
100 & 0 & 0 \
0 & 100 & 0 \
0 & 0 & 100
\end{bmatrix}$

第三次迭代时：
$U(3)^- =
\begin{bmatrix}
1 & -7.3366 & -1.0469 \
0 & 1 & 0.2405 \
0 & 0 & 1
\end{bmatrix}$；
$D(3)^- =
\begin{bmatrix}
0.0386 & 0 & 0 \
0 & 0.0074 & 0 \
0 & 0 & 17.1315
\end{bmatrix}$

$U(3)^+ =
\begin{bmatrix}
1 & -6.8463 & -1.0772 \
0 & 1 & 0.2442 \