In the standard EKF update, given a linearized measurement error (or residual) equation:

we naively need to compute the residual covariance matrix as follows:

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., , , and ). This motivates the need for a method to remove the feature 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 . We can compute it using QR decomposition as follows:

Multiplying the linearized measurement equation by the nullspace of the feature Jacobian from the left yields:

where we have employed the fact that and are orthonormal.

We now examine the dimensions of the involved matrices to appreciate the computation saving gained from this nullspace projection.

With that, we can have the following conclusion about the sizes when the nullspace is applied:

Finally, we perform the EKF update using the inferred measurement :

where the time index (subscript) refers to the prior estimate which was denoted before by symbol and corresponds to the posterior (or updated) estimate indicated before by .

Using Eigen 3 library, we perform QR decomposition to get the nullspace. Here we know that the size of is a 3x3, which corresponds to the size of the 3D point feature state.

Search for symbols, directories, files, pages or
modules. You can omit any prefix from the symbol or file path; adding a
: or / suffix lists all members of given symbol or
directory.

Use ↓
/ ↑ to navigate through the list,
Enter to go.
Tab autocompletes common prefix, you can
copy a link to the result using ⌘L while ⌘M produces a Markdown link.