166 lines
7.1 KiB
Plaintext
166 lines
7.1 KiB
Plaintext
/**
|
|
|
|
|
|
@page update-null MSCKF Nullspace Projection
|
|
|
|
|
|
In the standard EKF update, given a linearized measurement error (or residual) equation:
|
|
|
|
\f{align*}{
|
|
\tilde{\mathbf{z}}_{m,k}
|
|
&\simeq
|
|
\mathbf{H}_{x} \tilde{\mathbf{x}}_{k}
|
|
+ \mathbf{H}_{f} {}^G\tilde{\mathbf{p}}_f + \mathbf{n}_k
|
|
\f}
|
|
|
|
we naively need to compute the residual covariance matrix \f$\mathbf{P}_{zz}\f$ as follows:
|
|
|
|
\f{align*}{
|
|
\mathbf{P}_{zz}
|
|
&= \mathbb{E}\left[ \tilde{\mathbf{z}}_{m,k}{m,k} \tilde{\mathbf{z}}_{m,k}^\top \right] \\[5px]
|
|
&= \mathbb{E}\left[
|
|
(\mathbf{H}_{x}\tilde{\mathbf{x}}_{k}+\mathbf{H}_{f}{}^G\tilde{\mathbf{p}}_f+\mathbf{n}_k)
|
|
(\mathbf{H}_{x}\tilde{\mathbf{x}}_{k}+\mathbf{H}_{f}{}^G\tilde{\mathbf{p}}_f+\mathbf{n}_k)^\top
|
|
\right] \\[5px]
|
|
&= \mathbb{E}\Big[
|
|
\mathbf{H}_{x}\tilde{\mathbf{x}}_{k}\tilde{\mathbf{x}}_{k}^\top\mathbf{H}_{x}^\top
|
|
+\mathbf{H}_{x}\tilde{\mathbf{x}}_{k}{}^G\tilde{\mathbf{p}}_f^\top\mathbf{H}_{f}^\top
|
|
+\textcolor{red}{\mathbf{H}_{x}\tilde{\mathbf{x}}_{k}\mathbf{n}_k^\top} \nonumber\\[3px]
|
|
&\hspace{3cm}+
|
|
\mathbf{H}_{f}{}^G\tilde{\mathbf{p}}_f\tilde{\mathbf{x}}_{k}^\top\mathbf{H}_{x}^\top
|
|
+\mathbf{H}_{f}{}^G\tilde{\mathbf{p}}_f{}^G\tilde{\mathbf{p}}_f^\top\mathbf{H}_{f}^\top
|
|
+\mathbf{H}_{f}{}^G\tilde{\mathbf{p}}_f\mathbf{n}_k^\top \nonumber\\[3px]
|
|
&\hspace{3cm}+
|
|
\textcolor{red}{\mathbf{n}_k\tilde{\mathbf{x}}_{k}^\top\mathbf{H}_{x}^\top}
|
|
+\mathbf{n}_k{}^G\tilde{\mathbf{p}}_f^\top\mathbf{H}_{f}^\top
|
|
+\mathbf{n}_k\mathbf{n}_k^\top
|
|
\Big] \\[5px]
|
|
\empty
|
|
&=
|
|
\mathbf{H}_x\mathbb{E}\Big[\tilde{\mathbf{x}}_{k}\tilde{\mathbf{x}}_{k}^\top\Big]\mathbf{H}_x^\top
|
|
+\mathbf{H}_x\mathbb{E}\Big[\tilde{\mathbf{x}}_{k}{}^G\tilde{\mathbf{p}}_f^\top\Big]\mathbf{H}_f^\top
|
|
+\mathbf{H}_f\mathbb{E}\Big[{}^G\tilde{\mathbf{p}}_f\tilde{\mathbf{x}}_{k}^\top\Big]\mathbf{H}_x^\top
|
|
+ \mathbf{H}_f\mathbb{E}\Big[{}^G\tilde{\mathbf{p}}_f{}^G\tilde{\mathbf{p}}_f^\top\Big]\mathbf{H}_f^\top \nonumber\\[3px]
|
|
&\hspace{3.2cm}+
|
|
\mathbf{H}_f\mathbb{E}\Big[{}^G\tilde{\mathbf{p}}_f\mathbf{n}_k^\top\Big]
|
|
+\mathbb{E}\Big[\mathbf{n}_k{}^G\tilde{\mathbf{p}}_f^\top\Big]\mathbf{H}_f^\top
|
|
+\mathbb{E}\Big[\mathbf{n}_k\mathbf{n}_k^\top\Big] \\[5px]
|
|
\empty
|
|
&=
|
|
\mathbf{H}_x \mathbf{P}_{xx}\mathbf{H}_x^\top
|
|
+\mathbf{H}_x \mathbf{P}_{xf}\mathbf{H}_f^\top
|
|
+\mathbf{H}_f \mathbf{P}_{fx}\mathbf{H}_x^\top
|
|
+ \mathbf{H}_f \mathbf{P}_{ff}\mathbf{H}_f^\top \nonumber\\[3px]
|
|
&\hspace{2.3cm}+
|
|
\mathbf{H}_f \mathbf{P}_{fn}
|
|
+\mathbf{P}_{nf} \mathbf{H}_f^\top
|
|
+\mathbf{R}_d
|
|
\f}
|
|
|
|
However, there would be a big problem in visual-inertial odometry (VIO);
|
|
that is, we do not know what the prior feature covariance and it is coupled with both the state, itself, and the noise (i.e., \f$\mathbf{P}_{xf}\f$,\f$\mathbf{P}_{ff}\f$, and \f$\mathbf{P}_{nf}\f$).
|
|
This motivates the need for a method to remove the feature \f${}^G\tilde{\mathbf{p}}_f\f$ from the linearized measurement equation (thus removing the correlation between the measurement and its error).
|
|
|
|
|
|
To this end, we start with the measurement residual function by removing the "sensitivity" to feature error we compute and apply the left nullspace of the Jacobian \f$\mathbf{H}_{f}\f$. We can compute it using QR decomposition as follows:
|
|
|
|
\f{align*}{
|
|
\mathbf{H}_{f} =
|
|
\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}
|
|
|
|
|
|
Multiplying the linearized measurement equation by the nullspace of the feature Jacobian from the left yields:
|
|
|
|
\f{align*}{
|
|
\tilde{\mathbf{z}}_{m,k}
|
|
&\simeq
|
|
\mathbf{H}_{x} \tilde{\mathbf{x}}_{k}
|
|
+ \mathbf{Q_1}\mathbf{R_1}{}^G\tilde{\mathbf{p}}_f + \mathbf{n}_k \\[5px]
|
|
\empty
|
|
\Rightarrow~ \mathbf{Q_2}^\top\tilde{\mathbf{z}}_m
|
|
&\simeq
|
|
\mathbf{Q_2}^\top\mathbf{H}_{x} \tilde{\mathbf{x}}_{k}
|
|
+ \textcolor{red}{\mathbf{Q_2}^\top\mathbf{Q_1}\mathbf{R_1} {}^G\tilde{\mathbf{p}}_f} + \mathbf{Q_2}^\top\mathbf{n}_k \\[5px]
|
|
\empty
|
|
\Rightarrow~ \mathbf{Q_2}^\top\tilde{\mathbf{z}}_m
|
|
&\simeq
|
|
\mathbf{Q_2}^\top\mathbf{H}_{x} \tilde{\mathbf{x}}_{k} + \mathbf{Q_2}^\top\mathbf{n}_k \\[5px]
|
|
\empty
|
|
\Rightarrow~ \tilde{\mathbf{z}}_{o,k}
|
|
&\simeq
|
|
\mathbf{H}_{o,k}\tilde{\mathbf{x}}_{k} + \mathbf{n}_{o,k}
|
|
\f}
|
|
|
|
where we have employed the fact that \f$\mathbf Q_1\f$ and \f$\mathbf Q_2\f$ are orthonormal.
|
|
|
|
|
|
We now examine the dimensions of the involved matrices to appreciate the computation saving gained from this nullspace projection.
|
|
|
|
\f{align*}{
|
|
\textrm{size}(\mathbf{H}_{f}) &= 2n\times3 \textrm{~~where~}n\textrm{~is the number of uv measurements of this feature} \\[5px]
|
|
\textrm{size}({}^G\tilde{\mathbf{p}}_f) &= 3\times1 \\[5px]
|
|
\textrm{size}(\mathbf{H}_{x}) &= 2n\times15+6c \textrm{~~where~}c\textrm{~is the number of clones} \\[5px]
|
|
\textrm{size}(\tilde{\mathbf{x}}_{k}) &= 15+6c\times1 \textrm{~~where~}c\textrm{~is the number of clones} \\[5px]
|
|
\textrm{rank}(\mathbf{H}_{f}) &\leq \textrm{min}(2n,3) = 3 \textrm{~~where equality holds in most cases} \\[5px]
|
|
\textrm{nullity}(\mathbf{H}_{f}) &= \textrm{size}(\mathbf{x}) - \textrm{rank}(\mathbf{H}_{f}) = 2n-3 \textrm{~~assuming full rank}
|
|
\f}
|
|
|
|
With that, we can have the following conclusion about the sizes when the nullspace is applied:
|
|
|
|
\f{align*}{
|
|
\mathbf{Q_2}^\top\tilde{\mathbf{z}}_{m,k}
|
|
&\simeq
|
|
\mathbf{Q_2}^\top\mathbf{H}_{x} \tilde{\mathbf{x}}_{k} + \mathbf{Q_2}^\top\mathbf{n}_k \\[5px]
|
|
\empty
|
|
\Rightarrow~ (2n-3\times2n)(2n\times1)
|
|
&=
|
|
(2n-3\times2n)(2n\times15+6c)(15+6c\times1) \\
|
|
&\hspace{3.5cm}+ (2n-3\times2n)(2n\times1) \nonumber\\[5px]
|
|
\empty
|
|
\tilde{\mathbf{z}}_{o,k}
|
|
&\simeq
|
|
\mathbf{H}_{o,k}\tilde{\mathbf{x}}_{k} + \mathbf{n}_o \\[5px]
|
|
\empty
|
|
\Rightarrow~ (2n-3\times1)
|
|
&=
|
|
(2n-3\times15+6c)(15+6c\times1) + (2n-3\times1)
|
|
\f}
|
|
|
|
Finally, we perform the EKF update using the inferred measurement \f$\mathbf z_{o,k}\f$:
|
|
|
|
\f{align*}{
|
|
\hat{\mathbf{x}}_{k|k}
|
|
&= \hat{\mathbf{x}}_{k|k-1} + \mathbf{P}_{k|k-1} \mathbf{H}_{o,k}^\top (\mathbf{H}_{o,k} \mathbf{P}_{k|k-1} \mathbf{H}_{o,k}^\top + \mathbf{R}_o)^{-1}\tilde{\mathbf{z}}_{o,k} \\[5px]
|
|
\mathbf{P}_{k|k}
|
|
&= \mathbf{P}_{k|k-1} - \mathbf{P}_{k|k-1}\mathbf{H}_{o,k}^\top (\mathbf{H}_{o,k} \mathbf{P}_{k|k-1} \mathbf{H}_{o,k}^\top + \mathbf{R}_o)^{-1} \mathbf{H}_{o,k}\mathbf{P}_{k|k-1}^\top
|
|
\f}
|
|
|
|
where the time index (subscript) \f$k|k-1\f$ refers to the prior estimate which was denoted before by symbol \f$\ominus\f$
|
|
and \f$k|k\f$ corresponds to the posterior (or updated) estimate indicated before by \f$\oplus\f$.
|
|
|
|
|
|
|
|
|
|
|
|
@section implementation Implementation
|
|
|
|
Using Eigen 3 library, we perform QR decomposition to get the nullspace.
|
|
Here we know that the size of \f$\mathbf{Q}_1\f$ is \f$2n\times3\f$, which corresponds to the number of observations and size of the 3D point feature state.
|
|
|
|
@code{.cpp}
|
|
Eigen::ColPivHouseholderQR<Eigen::MatrixXd> qr(H_f.rows(), H_f.cols());
|
|
qr.compute(H_f);
|
|
Eigen::MatrixXd Q = qr.householderQ();
|
|
Eigen::MatrixXd Q1 = Q.block(0,0,Q.rows(),3);
|
|
Eigen::MatrixXd Q2 = Q.block(0,3,Q.rows(),Q.cols()-3);
|
|
@endcode
|
|
|
|
|
|
|
|
|
|
|
|
*/
|