initial commit

docs/update-compress.dox

@@ -0,0 +1,75 @@
+/**
+
+
+@page update-compress 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:
+
+\f{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
+\f}
+
+
+This QR decomposition can be performed again using [Givens rotations](https://en.wikipedia.org/wiki/Givens_rotation) (note that this operation in general is not cheap though).
+We apply this QR to the linearized measurement residuals to compress measurements:
+
+\f{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
+\f}
+
+
+As a result, the compressed measurement Jacobian will be of the size of the state,
+which will  signficantly reduce the EKF update cost:
+
+\f{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
+\f}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+*/