tutorialpoint.org

# Nonlinear Estimation

## 2 Literature review on quadrature based nonlinear estimation (cont'd...)

### 2.6 General filtering algorithm

Filter Initializing
$\bullet$ Initialize the filter with initial estimate $\hat{x}_{0 \vert 0}$, initial covariance $P_{x,0 \vert 0}$ and noise covariances $\hat{Q}_{k}$ and $\hat{R}_{k}$.
$\bullet$ Perform Cholesky decomposition of posterior error covariance to find it's square-root, such that \begin{equation*} P_{x,k-1 \vert k-1}=\zeta_{x,k-1 \vert k-1} \zeta_{x,k-1 \vert k-1}^T \end{equation*} $\bullet$ Evaluate quadrature points with the given mean and covariance \begin{equation*} x_{j,k-1 \vert k-1}=\zeta_{x,k-1 \vert k-1}\xi_{j}+\hat{x}_{k-1 \vert k-1} \end{equation*} $\bullet$ Propagate each one through the corresponding state transition function $\phi(\cdot)$ \begin{equation*} {x}_{j,k \vert k-1}=\phi(x_{j,k-1 \vert k-1},\hat{x}_{k-1 \vert k-1}) \end{equation*} $\bullet$ The mean can be predicted by computing the weighted mean of the propagated sigma-points \begin{equation*} \hat{x}_{k \vert k-1}=\sum_{j=1}^{Ps}w_{s,j}x_{j,k \vert k-1} \end{equation*} $\bullet$ Prior covariance can be computed as \begin{equation*} \begin{split} P_{x,k\vert k-1}=&\sum_{j=1}^{P_{s}}w_{s,j}({x}_{j,k \vert k-1}-\hat{x}_{k \vert k-1})({x}_{j,k \vert k-1}-\hat{x}_{k \vert k-1}^{T}+\hat{Q}_{k} \end{split} \end{equation*} Step 4 Measurement update
$\bullet$ Perform the Cholesky decomposition of prior covariance \begin{equation*} P_{x,k \vert k-1}=\zeta_{x,k \vert k-1}\zeta_{x,k \vert k-1}^{T} \end{equation*} $\bullet$ Evaluate the SGH points with given mean and covariance \begin{equation*} \tilde{x}_{j,k \vert k-1}=\zeta_{x,k \vert k-1}\xi_{j}+\hat{x}_{k \vert k-1} \end{equation*} $\bullet$ Propagate the SGH points through the given measurement function \begin{equation*} \begin{split} Y_{j,k \vert k-1}=\gamma(\tilde{x}_{j,k \vert k-1},\hat{x}_{j,k \vert k-1}) \end{split} \end{equation*} $\bullet$ The predicted measurement can be obtained as \begin{equation*} \begin{split} \hat{Y}_{k \vert k-1}=\sum_{j=1}^{P_{s}}w_{s,j}Y_{j,k \vert k-1} \end{split} \end{equation*}