2.6 Extended Kalman Filtering in SO(3)

Equation 40 could be used as the dynamics update in an extended Kalman filter (EKF), where $ \left(\mathbf{R}_{0},\boldsymbol{\Sigma}_{0}\right)$ is the prior state and $ \left(\mathbf{R}_{1},\boldsymbol{\Sigma}_{1}\right)$ is the dynamic model.

Note that Equation 42 is actually the EKF measurement update for the covariance and Equation 45 is the measurement update for the mean, assuming a trivial measurement Jacobian (identity matrix). The tangent vector $ \mathbf{v}$ is the innovation.

In this case of trivial measurement Jacobian, the Kalman gain $ \mathbf{K}$ is defined

$\displaystyle \mathbf{K}\equiv\boldsymbol{\Sigma}_{0}\left(\boldsymbol{\Sigma}_{0}+\boldsymbol{\Sigma}_{1}\right)^{-1}$ (46)

so that the Kalman update can be written in its standard form:


$\displaystyle \mathbf{R}_{c}$ $\displaystyle =$ $\displaystyle \mathbf{R}_{0}\oplus\left(\mathbf{K}\cdot\mathbf{v}\right)$ (47)
  $\displaystyle =$ $\displaystyle \exp\left(\mathbf{K}\cdot\mathbf{v}\right)\cdot\mathbf{R}_{0}$ (48)
$\displaystyle \boldsymbol{\Sigma}_{c}$ $\displaystyle =$ $\displaystyle \left(\mathbf{I}-\mathbf{K}\right)\cdot\boldsymbol{\Sigma}_{0}$ (49)

Labelling the above in the standard EKF framework, the state covariance is given by $ \boldsymbol{\Sigma}_{0}$ and the measurement noise is given by $ \boldsymbol{\Sigma}_{1}$. Note that Eq. 48 is mathematically identical to Eq. 45, and Eq. 49 is identical to Eq. 42.

The case of non-trivial measurement or dynamics Jacobians is a simple modification of the equations given here.

Ethan Eade 2012-02-16