75 lines
2.0 KiB
Plaintext
75 lines
2.0 KiB
Plaintext
/**
|
|
|
|
|
|
@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}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
*/ |