Notation

Let the mean and covariance be denoted by

\[ \begin{align} m_k & = \mathbb{E}\left(x_{t_k}\right) \\ P_k & = \mathbb{V}\left(x_{t_k}\right) \end{align} \]

General Idea

The general idea in Knudsen and Leth’s A New Continuous Discrete Unscented Kalman Filter is to approximate the Gaussian transition density by applying the Unscented Transform to the Euler-Maruyama discretization of the SDE.

Setup

Consider the Euler-Maruyama scheme approximating the path from \(x_{k}\) to \(x_{k+1}\) using \(L\) intermediate steps i.e.

\[ x_{k}^{(i+1)} = x_{k}^{(i)} + f(x_{k}^{(i)}) \, \Delta t_{i} + g(x_{k}^{(i)}) \, \Delta B_{i} \qquad \text{for} \quad i=0,1,2,...,L-1 \]

and let the sought solution be the final iterate i.e. \(x_{k+1} = x_{k}^{L}\). We can view this procedure as the mapping from the initial state value and the noise increments to the final iterate i.e.

\[ \begin{equation} x_{k+1} = \Phi(z_k) = \Phi(x_{k}, \Delta B_{0},\Delta B_{1},...,\Delta B_{L-1}) \end{equation} \]

We can use the Unscented Transform on this mapping to obtain an approximate Gaussian transition at \(x_{k+1}\), assuming we can generate the sigma points associated with \(z\). We just need the mean and covariance of \(z_k\), and these are easily obtained since all variables are independent and

\[ \Delta B_{i} \sim \mathcal{N}\left( 0 \, , \, I \Delta t_{i} \right) \]

The mean and covariance of \(z\) are given by:

\[ \begin{align} \mathbb{E}\left(z_k\right) = \begin{bmatrix} m_{k} \\ 0 \\ 0 \\ \vdots \\ 0 \end{bmatrix} && \mathbb{V}\left(z_k\right) = \begin{bmatrix} P_{k} & 0 & 0 & 0 & 0 \\ 0 & I\Delta t_{i} & 0 & 0 & 0 \\ 0 & 0 & I\Delta t_{i} & 0& 0 \\ 0 & 0 & 0 & I\Delta t_{i} & 0 \\ 0 & 0 & 0 & 0 & \ddots \end{bmatrix} \end{align} \]

The general form of the sigma-points is:

\[ \mathcal{X} = \left[\begin{array}{c|c|c} \mathbb{E}\left(z_k\right) & \mathbb{E}\left(z_k\right) + c \, \Lambda_k \, \text{diag}(\sqrt{\lambda_k}) & \mathbb{E}\left(z_k\right) - c \, \Lambda_k \, \text{diag}(\sqrt{\lambda_k}) \end{array}\right] \]

where \(\Lambda_k\) is a matrix of eigenvectors of \(\mathbb{V}\left(z_k\right)\), and \(\lambda_k\) is a vector of the eigenvalues, and \(c\) is a constant. In this case we will therefore get the sigma-point structure

\[ \mathcal{X} = \left[ \begin{array}{c|cc|cc|cc|c|cc} m_k & m_k + cQ_k & m_k - cQ_k & m_k & m_k & m_k & m_k & \cdots & m_k & m_k \\ 0 & 0 & 0 & cI\sqrt{\Delta t_0} & -cI\sqrt{\Delta t_0} & 0 & 0 & \cdots & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & cI\sqrt{\Delta t_1} & -cI\sqrt{\Delta t_1} & \cdots & 0 & 0 \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & \cdots & cI\sqrt{\Delta t_{L-1}} & -cI\sqrt{\Delta t_{L-1}} \end{array} \right] \] with \(Q_k = \Lambda_k \, \text{diag}(\sqrt{\lambda_k})\).

We form in total \(N_{s} = 2N+1\) sigma points, where \(N = n+mL\) is the dimension of \(z\), with \(n\) and \(m\) being the dimensions of \(x \in R^{n}\) and \(dB \in R^{m}\), and where \(L\) was the number of intermediate discretization steps.

We push these sigma points through \(\Phi\) in order to obtain our approximate mean and covariance at the next iterate. In summary the algorithm reconstructs these as:

\[ m_{k+1 \mid k} = \sum_{i=0}^{2N} w_{i} \Phi(\mathcal{X}_{i}) \] \[ P_{k+1 \mid k} = \sum_{i=0}^{2N} w_{i} \left( \Phi(\mathcal{X}_{i}) - m_{k+1 \mid k} \right)\left( \Phi(\mathcal{X}_{i}) - m_{k+1 \mid k} \right)^{T} \]

f3c <- seq_len(2*n+1)
for(i in seq_len(L)){
  X[i,f3c] <- X[i,f3c] + apply(X[i,f3c],2,f) * dt
}

create.sigma.points <- function(x,p,L){
  
  
  e <- eigen(p)
  
  
}