insCF

Estimate pose and velocity using complementary filter

Since R2026a

expand all in page

    Description

    The insCF object implements a complementary filter. You can use the filter to fuse data from an accelerometer, magnetometer, gyroscope, and GPS, modeled by the insCFAccelerometer, insCFMagnetometer , insCFGyroscope, and insCFGPS objects respectively, to estimate position, orientation, and velocity. The toolbox also provides the insCFMotionOrientation and insCFMotionPose objects as motion models. For more information about using the inertial sensors and motion models with the insCF object, see the sensor1,...,sensorN and motionModel input arguments.

    For ease of use, the sensor gain is the only tunable parameter of the insCF filter. You can use the gainparts function to get and set the gain value of a sensor. For information on filters with more tunable parameters, see Choose Inertial Sensor Fusion Filters.

    Creation

    Syntax

    filter = insCF
    filter = insCF(sensor1,...,sensorN)
    filter = insCF(___,motionModel)
    filter = insCF(___,options)

    Description

    filter = insCF creates an insCF object that implements a complementary filter with default property values. This filter can estimate orientation using accelerometer and gyroscope sensor data.

    example

    filter = insCF(sensor1,...,sensorN) specifies the inertial sensor models from which to fuse the data. The filter saves the specified inertial sensor models and their corresponding sensor names in the Sensors and SensorNames properties, respectively. Each specified inertial sensor model must be unique.

    example

    filter = insCF(___,motionModel) specifies the motion model in addition to any combination of input arguments from previous syntaxes. The filter saves the specified motion model in the MotionModel property.

    example

    filter = insCF(___,options) configures the filter using the specified insCFOptions object options.

    example

    Input Arguments

    expand all

    Sensor models to fuse in the complementary filter, specified as two or more of these inertial sensor models that includes at least one prediction and one correction sensor model.

    ObjectSensor DataType of Sensor
    insCFGyroscopeGyroscope dataOrientation prediction sensor
    insCFMagnetometerMagnetometer dataOrientation correction sensor
    insCFAccelerometer Accelerometer data

    • Orientation correction sensor

    • Position and velocity prediction sensor

    insCFGPS GPS dataPosition correction sensor

    • If the value of MotionModel property is set to insCFMotionOrientation, for modeling orientation, you must specify insCFGyroscope for the orientation prediction phase of sensor fusion and one of the orientation correction sensor models, insCFAccelerometer or insCFMagnetometer, for the orientation correction phase.

    • If the value of MotionModel property is set to insCFMotionPose, for modeling pose (orientation and position) and velocity, in addition to the requirements for the orientation prediction and correction phases of sensor fusion, you must specify insCFAccelerometer for the position and velocity prediction phase and insCFGPS for the position correction phase.

    This argument sets the Sensors property and the SensorNames property.

    Motion model used in the complementary filter, specified as an insCFMotionOrientation object or an insCFMotionPose object.

    The default value of this argument depends on the inertial sensor models in the insCF object.

    • If Sensors property does not contain an insCFGPS object, then the default motion model is an insCFMotionOrientation object.

    • If Sensors property contains an insCFGPS object, then the default motion model depends on the presence of insCFAccelerometer in the insCF object. If Sensors property contains insCFAccelerometer object, then the default motion model is an insCFMotionPose object. Else, the default motion model is an insCFMotionOrientation object.

    This argument sets the MotionModel property.

    Filter options, specified as an insCFOptions object. Use this argument to set the data type of the state and internal variables, as well as the reference frame, of the complementary filter.

    This argument sets the ReferenceFrame property.

    Output Arguments

    expand all

    Complementary filter to fuse IMU and GPS data, returned as an insCF object.

    Properties

    expand all

    This property is read-only after object creation. To set this property, use the sensor1,...,sensorN argument when calling the insCF function.

    Sensor models fused in the complementary filter, represented as a cell array of two or more of these inertial sensor models.

    ObjectSensor DataType of Sensor
    insCFGyroscopeGyroscope dataOrientation prediction sensor
    insCFMagnetometerMagnetometer dataOrientation correction sensor
    insCFAccelerometer Accelerometer data

    • Orientation correction sensor

    • Position and velocity prediction sensor

    insCFGPS GPS dataPosition correction sensor

    This property is read-only.

    Names of the sensors, represented as a cell array of character vectors. The names are automatically derived from the sensor objects specified in the Sensors property and are listed in the same order. The name for each sensor is determined by its object type.

    Sensor ObjectSensor Name
    insCFGyroscope'Gyroscope'
    insCFMagnetometer'Magnetometer'
    insCFAccelerometer'Accelerometer'
    insCFGPS'GPS'

    Data Types: cell

    This property is read-only after object creation. To set this property, use the motionModel argument when calling the insCF function.

    Motion model used in the complementary filter, represented as an insCFMotionOrientation object or an insCFMotionPose object.

    The default value of this property depends on the inertial sensor models in the insCF object.

    • If Sensors property does not contain an insCFGPS object, then the default motion model is an insCFMotionOrientation object.

    • If Sensors property contains an insCFGPS object, then the default motion model depends on the presence of insCFAccelerometer in the insCF object. If Sensors property contains insCFAccelerometer object, then the default motion model is an insCFMotionPose object. Else, the default motion model is an insCFMotionOrientation object.

    This property is read-only after object creation. To set this property, use the options argument when calling the insCF function.

    Reference frame of the complementary filter, represented as "NED" for the north-east-down frame or "ENU" for the east-north-up frame.

    Object Functions

    statepartsGet or set value of state part in insCF
    estimateStatesEstimate states using insCF
    gainpartsGet or set gain part value for sensor in insCF
    copyCreate copy of insCF
    resetReset state parts of insCF

    Examples

    collapse all

    Open Live Script

    Create complementary filters, using the insCF filter object, with different configurations.

    Create Complementary Filter for Estimating Orientation

    Create a complementary filter as a default insCF object. By default, the filter can fuse the data from an accelerometer and a gyroscope, assuming orientation-only motion, and thus capable of estimating orientation.

    filter1 = insCF
    filter1 = 
  insCF with properties:

           Sensors: {[1×1 insCFAccelerometer]  [1×1 insCFGyroscope]}
       SensorNames: {'Accelerometer'  'Gyroscope'}
       MotionModel: [1×1 insCFMotionOrientation]
    ReferenceFrame: 'NED'

    Create Complementary Filter for Estimating Pose and Velocity

    Create an insCF filter object that can fuse the data from an accelerometer, a gyroscope, and a GPS. Specify the insCFMotionPose motion model to enable the filter to estimate pose and velocity. 

    filtr2 = insCF(insCFAccelerometer,insCFGyroscope,insCFGPS,insCFMotionPose)
    filtr2 = 
  insCF with properties:

           Sensors: {[1×1 insCFAccelerometer]  [1×1 insCFGyroscope]  [1×1 insCFGPS]}
       SensorNames: {'Accelerometer'  'Gyroscope'  'GPS'}
       MotionModel: [1×1 insCFMotionPose]
    ReferenceFrame: 'NED'

    Create Complementary Filter with ENU Reference Frame

    Create another insCF filter object that can fuse the data from an accelerometer, a gyroscope, and a GPS, but specify the reference frame of the filter as the east-north-up (ENU) frame by using an insCFOptions object. Note that, because the accelerometer data predicts pose and GPS data corrects pose, the filter uses the insCFMotionPose motion model by default.

    options = insCFOptions(ReferenceFrame="ENU");
filter3 = insCF(insCFAccelerometer,insCFGyroscope,insCFGPS)
    filter3 = 
  insCF with properties:

           Sensors: {[1×1 insCFAccelerometer]  [1×1 insCFGyroscope]  [1×1 insCFGPS]}
       SensorNames: {'Accelerometer'  'Gyroscope'  'GPS'}
       MotionModel: [1×1 insCFMotionPose]
    ReferenceFrame: 'NED'
    Open Live Script

    Estimate orientation from recorded IMU data by using a complementary filter, represented by an insCF filter object.

    Load the rpy_9axis.mat file into the workspace. The file contains recorded accelerometer, gyroscope, and magnetometer sensor data from a device oscillating in pitch (around the y-axis), yaw (around the z-axis), and roll (around the x-axis). The file also contains the sample rate of the recording. Extract the sensor data and sample rate from the loaded workspace variable.

    ld = load("rpy_9axis.mat");
accelerometerData = ld.sensorData.Acceleration;
gyroscopeData = ld.sensorData.AngularVelocity;
magnetometerData = ld.sensorData.MagneticField;
imuSampleRate = ld.Fs; % Hz

    Create a timetable from the accelerometer, gyroscope, and magnetometer data.

    imuDataTT = timetable(accelerometerData,gyroscopeData,magnetometerData,'SampleRate',imuSampleRate,'StartTime',seconds(1/imuSampleRate),'VariableNames',{'Accelerometer', 'Gyroscope', 'Magnetometer'});

    Create an insCF filter object using sensor models for accelerometer, magnetometer, and gyroscope. To model orientation, specify insCFMotionOrientation as the motion model.

    acc = insCFAccelerometer;
mag = insCFMagnetometer;
gyro = insCFGyroscope;
motionModel = insCFMotionOrientation;
filter = insCF(acc,mag,gyro,motionModel);

    Specify the initial orientation of your sensors. You can obtain the initial orientation by using the ecompass function with the accelerometer and magnetometer data at the first time step.

    initialOrientation = [0.006727036930852 -0.131203115007920 -0.058108427699335 -0.989628162602834];
stateparts(filter,"Orientation",initialOrientation)

    Specify the sensor gains. The sensor gain value must be between 0 and 1.

    gainparts(filter,"Accelerometer",0)
gainparts(filter,"Magnetometer",0.01)

    Fuse the accelerometer, gyroscope, and magnetometer data using the insCF filter.

    outTT = estimateStates(filter,imuDataTT);

    Visualize the estimated orientation.

    t = outTT.Properties.RowTimes;
plot(t,eulerd(outTT.Orientation,"ZYX","frame"));
title("Orientation Estimate");
legend("Z-rotation","Y-rotation","X-rotation");
xlabel("Time");
ylabel("Degrees");

    Figure contains an axes object. The axes object with title Orientation Estimate, xlabel Time, ylabel Degrees contains 3 objects of type line. These objects represent Z-rotation, Y-rotation, X-rotation.

    Open Live Script

    Estimate pose (orientation and position) and velocity from IMU and GPS data by using a complementary filter, represented by an insCF filter object.

    Load the racetrackINSCFDataset.mat file, which contains a timetable with simulated accelerometer, gyroscope, magnetometer, and GPS sensor data for a racetrack-like trajectory, into the workspace. The file also contains the sample rate of the data.

    load("raceTrackINSCFDataset.mat")

    Create an insCF filter object that includes the accelerometer, gyroscope, magnetometer, and GPS sensor objects, as well as an insCFMotionPose motion model object for modeling pose.

    % Sensor object for predicting orientation
gyro = insCFGyroscope;
% Sensor objects for correcting orientation
accel = insCFAccelerometer;
mag = insCFMagnetometer;
% Sensor object for correcting position
gps = insCFGPS(ReferenceLocation=localOrigin);

% Pose motion model
motModel = insCFMotionPose;

% Filter object
filt = insCF(accel,mag,gyro,gps,motModel);

    Specify the initial position, velocity, and orientation from the measurement data.

    % Set initial position
stateparts(filt,"Position",initPos)
% Set initial orientation
stateparts(filt,"Orientation",initOrient)
% Set initial velocity
stateparts(filt,"Velocity",initVel)

    Specify the sensor gains.

    % Set Accelerometer gain
gainparts(filt,"Accelerometer",9.6335e-09)
% Set Magnetometer gain
gainparts(filt,"Magnetometer",0.01)
% Set GPS gain
gainparts(filt,"GPS",0.039246)

    Fuse the sensor data using the filter. The filter estimates pose (orientation and position) and velocity. Extract the estimated position.

    outTT = estimateStates(filt,sensorData);
estPos = outTT.Position;

    Calculate the position RMS error.

    posErr = truePos - estPos; 
pRMS = sqrt(mean(posErr.^2));
fprintf("Position RMS Error:\tX: %.3f, Y: %.3f, Z: %.3f (meters) \n", pRMS(1),pRMS(2),pRMS(3))
    Position RMS Error:	X: 0.372, Y: 0.493, Z: 0.303 (meters)

    Visualize the estimated pose against the ground truth and GPS data.

    figure
plot(truePos(:,1),truePos(:,2),".")
hold on
plot(estPos(:,1),estPos(:,2),"r.-")
gpsLocal = lla2ned(sensorData.GPS,localOrigin,"flat");
plot(gpsLocal(:,1),gpsLocal(:,2),".")

title("Pose Estimate")
legend("Ground Truth","Estimated","GPS")

grid on
xlabel("N (m)")
ylabel("E (m)")
axis equal

    Figure contains an axes object. The axes object with title Pose Estimate, xlabel N (m), ylabel E (m) contains 3 objects of type line. One or more of the lines displays its values using only markers These objects represent Ground Truth, Estimated, GPS.

    References

    [1] Vasconcelos, José F., Bruno Cardeira, Carlos Silvestre, Paulo Oliveira, and Pedro Batista. “Discrete-Time Complementary Filters for Attitude and Position Estimation: Design, Analysis and Experimental Validation.” IEEE Transactions on Control Systems Technology 19, no. 1 (2011): 181–98. https://doi.org/10.1109/TCST.2010.2040619.

    Extended Capabilities

    expand all

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

    Version History

    Introduced in R2026a

    See Also

    Objects

    Functions