Kalman with eigen

https://github.com/mherb/kalmanT 是一个基于eigin3的Kalman-Filter，包含了Extended Kalman Filter (EKF)、Square Root Extended Kalman Filter (SR-EKF)、Unscented Kalman Filter (UKF)、Square Root Unscented Kalman Filter (SR-UKF)，4种Kalman filter。

Cholesky分解

Hermitian matrix（埃尔米特矩阵，厄米矩阵，自伴随矩阵），是共轭对称的方阵。

$$\left[\begin{matrix} 3 & 2+i \\ 2-i & 1 \end{matrix}\right]$$

正定矩阵是Hermitian matrix的一种。
设A是一个n阶厄米特正定矩阵(Hermitian positive-definite matrix)。Cholesky分解的目标是：
设$A = LL^{T}$，得到：（其中$A_{21}$是一个列向量，$A_{22}$是一个n-1阶的方阵）

$$\left[ \begin{matrix} a_{11}&A_{21}^{T}\\ A_{21}&A_{22}\\ \end{matrix} \right] = \left[ \begin{matrix} l_{11}&0\\ L_{21}&L_{22}\\ \end{matrix} \right] \left[ \begin{matrix} l_{11}&L_{21}^{T}\\ 0&L_{22}^{T}\\ \end{matrix} \right]= \left[ \begin{matrix} l_{11}^{2}&l_{11}L_{21}^{T}\\ l_{11}L_{21}&L_{21}L_{21}^{T}+L_{22}L_{22}^{T}\\ \end{matrix} \right]$$

其中，未知量$l_{11},L_{21},L_{22}$，这3个未知量的求解公式是：

$$l_{11} = \sqrt {a_{11}}，L_{21} = \frac {1}{l_{11}}A_{21}，L_{22}L_{22}^{T} = A_{22} - L_{21}L_{21}^{T}$$

设$A_{22}’ = A_{22} - L_{21}L_{21}^{T}$，则化简为$A_{22}’ = L_{22}L_{22}^{T}$，可以继续Cholesky分解，被分解的矩阵是A的右下角的n-1阶子方阵。所以这个算法具有递归性质。

举个例子：

$$A = \left[ \begin{matrix} 25&15&-5\\ 15&18&0\\ -5&0&11\\ \end{matrix} \right] = \left[ \begin{matrix} l_{11}&0&0\\ l_{21}&l_{22}&0\\ l_{31}&l_{32}&l_{33}\\ \end{matrix} \right] \left[ \begin{matrix} l_{11}&l_{21}&l_{31}\\ 0&l_{22}&l_{32}\\ 0&0&l_{33}\\ \end{matrix} \right]$$

根据公式，有：

$$l_{11} = \sqrt { a_{11} } = 5$$ $$L_{21} = \frac {1}{l_{11}}A_{21} = \frac {1}{5} \left[ \begin{matrix} 15\\ -5\\ \end{matrix} \right] = \left[ \begin{matrix} 3\\ -1\\ \end{matrix} \right]$$ $$A_{22} - L_{21}L_{21}^{T} = L_{22}L_{22}^{T}$$ $$A_{22} - L_{21}L_{21}^{T} = L_{22}L_{22}^{T}$$ $$\left[ \begin{matrix} 18&0\\ 0&11\\ \end{matrix} \right] - \left[ \begin{matrix} 3\\ -1\\ \end{matrix} \right] \left[ \begin{matrix} 3&-1\\ \end{matrix} \right] = \left[ \begin{matrix} l_{22}&0\\ l_{32}&l_{33}\\ \end{matrix} \right] \left[ \begin{matrix} l_{22}&l_{32}\\ 0&l_{33}\\ \end{matrix} \right]$$ $$\left[ \begin{matrix} 9&3\\ 3&10\\ \end{matrix} \right] = \left[ \begin{matrix} l_{22}&0\\ l_{32}&l_{33}\\ \end{matrix} \right] \left[ \begin{matrix} l_{22}&l_{32}\\ 0&l_{33}\\ \end{matrix} \right]$$

(注意，这里已经是n-1阶的Cholesky分解)

$$l_{22} = \sqrt { 9 } = 3$$ $$l_{32} = \frac {1}{3}3 = 1$$ $$10 = l_{32}^{2} + l_{33}^{2} = 1 + l_{33}^{2}$$ $$l_{33} = \sqrt {10 - 1} = 3$$

综上

$$A = \left[ \begin{matrix} 25&15&-5\\ 15&18&0\\ -5&0&11\\ \end{matrix} \right] = \left[ \begin{matrix} 5&0&0\\ 3&3&0\\ -1&1&3\\ \end{matrix} \right] \left[ \begin{matrix} 5&3&-1\\ 0&3&1\\ 0&0&3\\ \end{matrix} \right]$$

对矩阵的Cholesky分解，就像对实数的求平方根。根据协方差矩阵的定义，Cholesky分解可以反求期望。