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

$$\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:

$$\mathbf{R}_{c} = \mathbf{R}_{0}\oplus\left(\mathbf{K}\cdot\mathbf{v}\right)$$ (47)

$$= \exp\left(\mathbf{K}\cdot\mathbf{v}\right)\cdot\mathbf{R}_{0}$$ (48)

$$\boldsymbol{\Sigma}_{c} = \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.