fusemvo

Correct states using monocular visual odometry for insfilterErrorState

Syntax

[pResidual,oResidual,resCov] = fusemvo(FUSE,position,positionCovariance,ornt,orntCovariance)

Description

[pResidual,oResidual,resCov] = fusemvo(FUSE,position,positionCovariance,ornt,orntCovariance) fuses position and orientation data from monocular visual odometry (MVO) measurements to correct the state and state estimation error covariance.

Examples

Fuse Monocular Visual Odometry Data from insfilterErrorState

Create an insfilterErrorState object and display its state.

filter = insfilterErrorState;
disp(filter.State')

     1     0     0     0     0     0     0     0     0     0     0     0     0     0     0     0     1

Define a MVO reading and its covariance. Fuse the reading.

position = [10 10 10]

position = 1×3

    10    10    10

positionCovariance = eye(3);
orientation = quaternion.ones;
orientationCovariance = 0.1*[1 1 1];
[res,resCovariance] = fusemvo(filter,position,positionCovariance, ...
    orientation,orientationCovariance)

res = 1×3

    10    10    10

resCovariance = 1×3

     0     0     0

Display the filter state.

disp(filter.State')

    0.7220    0.3995    0.3995    0.3995    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    0.8824    1.8824

Input Arguments

FUSE — INS filter object

insfilterErrorState

insfilterErrorState, specified as an object.

position — Position of camera in local NED coordinate system (m)

3-element row vector

Position of camera in the local NED coordinate system in meters, specified as a real finite 3-element row vector.

Data Types: single | double

positionCovariance — Position measurement covariance of MVO (m²)

scalar | 3-element vector | 3-by-3 matrix

Position measurement covariance of MVO in m², specified as a scalar, 3-element vector, or 3-by-3 matrix.

Data Types: single | double

ornt — Orientation of camera with respect to local NED coordinate system

scalar quaternion | rotation matrix

Orientation of the camera with respect to the local NED coordinate system, specified as a scalar quaternion or 3-by-3 rotation matrix. The quaternion or rotation matrix is a frame rotation from the NED coordinate system to the current camera coordinate system.

Data Types: quaternion | single | double

orntCovariance — Orientation measurement covariance of monocular visual odometry (rad²)

scalar | 3-element vector | 3-by-3 matrix

Orientation measurement covariance of monocular visual odometry in rad², specified as a scalar, 3-element vector, or 3-by-3 matrix.

Data Types: single | double

Output Arguments

pResidual — Position residual

1-by-3 vector of real values

Position residual, returned as a 1-by-3 vector of real values in m.

oResidual — Rotation vector residual

1-by-3 vector of real values

Rotation vector residual, returned as a 1-by-3 vector of real values in radians.

resCov — Residual covariance

6-by-6 matrix of real values

Residual covariance, returned as a 6-by-6 matrix of real values.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019a