Nonlinear Estimation
2 Literature review on quadrature based nonlinear estimation (cont'd...)
2.6 General filtering algorithm (cont'd...)
$\bullet$ Calculate innovation covariance
\begin{equation*}
\begin{split}
P_{y,k \vert k-1}=&\sum_{j=1}^{P_{s}}w_{s,j}(Y_{j,k \vert k-1}-\hat{Y}_{k \vert k-1}(Y_{j,k \vert k-1}-\hat{Y}_{k \vert k-1}^{T}+\hat{R}_{k}
\end{split}
\end{equation*}
$\bullet$ Calculate cross-covariance
\begin{equation*}
\begin{split}
P_{xy,k \vert k-1}=&\sum_{j=1}^{P_{s}}w_{s,j}(\tilde{x}_{j,k \vert k-1}-\hat{x}_{k \vert k-1}) (Y_{j,k \vert k-1}-\hat{Y}_{k \vert k-1})^{T}
\end{split}
\end{equation*}
$\bullet$ Calculate Kalman gain
\begin{equation*}
K_{k}=P_{xy,k \vert k-1}P_{y,k \vert k-1}^{-1}
\end{equation*}
$\bullet$ Compute the posterior mean
\begin{equation*}
\hat{x}_{k \vert k}=\hat{x}_{k \vert k-1}+K_{k}(y_{k}-\hat{Y}_{k \vert k-1})
\end{equation*}
$\bullet$ Compute the posterior covariance
\begin{equation*}
P_{x,k \vert k}=P_{x,k\vert k-1}-K_{k}P_{y,k \vert k-1}K_{k}^{T}
\end{equation*}
where $P_s$ is the number of quadrature points.
< Prev.Page
1   2  
3   4  5
  6   7   8
  9   10   11
  12   13   14
  15   16   17
  18   19
  Next page>