ov_core: Measurement Compression

One of the most costly opeerations in the EKF update is the matrix multiplication. To mitigate this issue, we perform the thin QR decomposition of the measurement Jacobian after nullspace projection:

$\begin{align*} \mathbf{H}_{o,k} = \begin{bmatrix} \mathbf{Q_1} & \mathbf{Q_2} \end{bmatrix} \begin{bmatrix} \mathbf{R_1} \\ \mathbf{0} \end{bmatrix} = \mathbf Q_1 \mathbf R_1 \end{align*}$

This QR decomposition can be performed again using Givens rotations (note that this operation in general is not cheap though). We apply this QR to the linearized measurement residuals to compress measurements:

$\begin{align*} \tilde{\mathbf{z}}_{o,k} &\simeq \mathbf{H}_{o,k}\tilde{\mathbf{x}}_{k} + \mathbf{n}_o \\[5px] \empty \tilde{\mathbf{z}}_{o,k} &\simeq \mathbf Q_1 \mathbf R_1 \tilde{\mathbf{x}}_{k} + \mathbf{n}_o \\[5px] \empty \mathbf{Q_1}^\top \tilde{\mathbf{z}}_{o,k} &\simeq \mathbf{Q_1}^\top \mathbf{Q_1} \mathbf{R_1} \tilde{\mathbf{x}}_{k} + \mathbf{Q_1}^\top \mathbf{n}_o \\[5px] \empty \mathbf{Q_1}^\top\tilde{\mathbf{z}}_{o,k} &\simeq \mathbf{R_1} \tilde{\mathbf{x}}_{k} + \mathbf{Q_1}^\top\mathbf{n}_o \\[5pt] \empty \Rightarrow~ \tilde{\mathbf{z}}_{n,k} &\simeq \mathbf{H}_{n,k} \tilde{\mathbf{x}}_{k} + \mathbf{n}_n \end{align*}$

As a result, the compressed measurement Jacobian will be of the size of the state, which will signficantly reduce the EKF update cost:

$\begin{align*} \hat{\mathbf{x}}_{k|k} &= \hat{\mathbf{x}}_{k|k-1} + \mathbf{P}_{k|k-1} \mathbf{H}_{n,k}^\top (\mathbf{H}_{n,k} \mathbf{P}_{k|k-1} \mathbf{H}_{n,k}^\top + \mathbf{R}_n)^{-1}\tilde{\mathbf{z}}_{n,k} \\[5px] \empty \mathbf{P}_{k|k} &= \mathbf{P}_{k|k-1} - \mathbf{P}_{k|k-1}\mathbf{H}_{n,k}^\top (\mathbf{H}_{n,k}\mathbf{P}_{k|k-1} \mathbf{H}_{n,k}^\top + \mathbf{R}_n)^{-1} \mathbf{H}_{n,k} \mathbf{P}_{k|k-1}^\top \end{align*}$