fusemvo

Correct states using monocular visual odometry for insfilterErrorState

collapse all in page

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.

example

Examples

collapse all

Open Live Script

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

collapse all

insfilterErrorState, specified as an object.

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

Data Types: single | double

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

Data Types: single | double

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

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

Data Types: single | double

Output Arguments

collapse all

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

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

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

Extended Capabilities

expand all

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

Version History

Introduced in R2019a

See Also

|