50.7.1 Observation of the error state (filter correction)

Parents: 50.3 Error-State Kalman Filter ,  50.5 eskf update / fusing imu with complementary sensory data

Source: Solà 2017 Quaternion kinematics for ESKF

Given is a non-IMU sensor with the measurement function [ Solà , Markley ] $$ \mathbf{y} = h(\mathbf{x}_t) + v $$

where $\mathbf{x}_t$ is the true state and $v$ is a white Gaussian noise $$ v \sim \mathcal{N}\left\lbrace 0, \mathbf{V}\right\rbrace $$

Source:  Markley 2014

If the measurements are given in quaternion form:

• we can directly calculate the orientation error between measured orientation and estimated orientation
• this becomes our ‘measured’ angular error $\mathbf{y}_k = \delta \mathbf{\theta}_k$  which can be used to calculate the residual term $$ \mathbf{q}_k \otimes \hat{\mathbf{q}}k^{-1} = \delta \mathbf{q} (\delta \mathbf{\theta}k) $$ [JPL convention]: $$\mathbf{q}\text{meas} \otimes \mathbf{q}\text{est}^* = \delta\mathbf{\theta}$$

Note: the measurement update should agree closely with the measurement residual calculation. therefore the measurement model should use the same parametrisation for $\delta \mathbf{q}$ as is used in the reset, s. Rotation error representation

Using  Gibbs / Rodrigues parameter representation for rotations :
$$ \mathbf{y}_k = \delta \mathbf{\theta}_k = 2 \dfrac{\left( \mathbf{q}_k \otimes \hat{\mathbf{q}}k^{-1} \right){1:3}} {\left( \mathbf{q}_k \otimes \hat{\mathbf{q}}_k^{-1} \right)_4} $$

Advantage: the observation model is insensitive to the sign ambiguity in the measured quaternion $$ \begin{aligned} & \quad \mathbf{h}_k + \mathbf{H}_k \left[ \begin{array}{c} \delta\mathbf{\hat{\theta}}_k^- \
\Delta \mathbf{\hat{\xi}}_k^- \end{array} \right] \
&= 2 \dfrac{\left( \mathbf{q}_k \otimes \hat{\mathbf{q}}k^{-1} \right){1:3}} {\left( \mathbf{q}_k \otimes \hat{\mathbf{q}}_k^{-1} \right)_4}

• \left[ \begin{array}{cc} \mathbf{I}3 & \mathbf{0}{3\times n} \end{array} \right] \left[ \begin{array}{c} \delta\mathbf{\hat{\theta}}_k^- \
  \Delta \mathbf{\hat{\xi}}_k^- \end{array} \right]\
  &= \delta \mathbf{\hat{\theta}}_k^- \end{aligned} $$

Residual becomes $y_k - \delta\theta_k$