## Fuse Inertial Sensor Data Using `insEKF`

-Based Flexible Fusion
Framework

The `insEKF`

filter object provides a flexible framework that you can use to
fuse inertial sensor data. You can fuse measurement data from various inertial sensors by
selecting or customizing the sensor models used in the filter, and estimate different
platform states by selecting or customizing the motion model used in the filter. The `insEKF`

object is based on a continuous-discrete extended Kalman filter, in
which the state prediction step is continuous, and the measurement correction or fusion step
is discrete.

Overall, the object contains 7 properties and 13 object functions. The object also interacts with other objects and functions to a support regular filter workflow, as well as filter tuning.

### Object Properties

The `insEKF`

object maintains major filter variables and
information of the motion model and sensor models using these properties:

`State`

— Platform state saved in the filter, specified as a vector. This property maintains the whole state vector, including the platform states defined by the motion model, such as orientation, position, and velocity, as well as the sensor states defined by sensor models, such as sensor biases.You can interact with this property in various ways:

Display the state information saved in the vector by using the

`stateinfo`

object function.Directly specify this property. This may not be convenient since the dimension of the state vector can be large.

Get or set a part or component of the state vector by using the

`stateparts`

object function.

`StateCovariance`

— State estimate error covariance, specified as a matrix. This property maintains the whole covariance matrix corresponding to the state vector. You can interact with this property in various ways:Directly specify this property. This may not be convenient since the dimension of the covariance matrix can be large.

Get or set a part of the covariance matrix by using the

`statecovparts`

object function.

`AdditiveProcessNoise`

— Additive process noise covariance, specified as a matrix. This property maintains the whole additive process noise covariance matrix corresponding to the state vector.`MotionModel`

— Motion model to predict the filter state, specified as an object. You can use one of these options to specify this property when constructing the filter:`insMotionOrientation`

— Model orientation-only platform motion assuming a*constant angular velocity*. Using this object adds the quaternion and angular velocity state to the`State`

property.`insMotionPose`

— Model 3-D motion assuming*constant angular velocity*and*constant linear acceleration*. Using this model adds the quaternion, angular velocity, position, linear velocity, and acceleration state to the`State`

property.`positioning.INSMotionModel`

— An interface class to implement a motion model object used with the filter. To customize a motion model object, you must inherit from this interface class and implement at least two methods:`modelstates`

and`stateTransition`

.

`Sensors`

— Sensor models that model the corresponding sensor measurement based on the platform state, specified as a cell array of INS sensor objects. You can specify each INS sensor object using one of these options:`insAccelerometer`

— Model accelerometer readings, including the gravitational acceleration and sensor bias by default. The model also includes the sensor acceleration if the`State`

property of the filter includes the`Acceleration`

state.`insMagnetometer`

— Model magnetometer readings, including the geomagnetic vector and the sensor bias.`insGyroscope`

— Model gyroscope readings, including the angular velocity vector and the sensor bias.`insGPS`

— Model GPS readings, including the latitude, longitude, and altitude coordinates. If you fuse velocity data from the GPS sensor, the object additionally models velocity measurements.`positioning.INSSensorModel`

— An interface class to implement a sensor model object used with the filter. To customize a sensor model object, you must inherit from this interface class and implement at least the`measurement`

method.

`SensorNames`

— Names of the sensors added to the filter, specified as a cell array of character vector. The filter object assigns default names to the added sensors. These sensor names are useful when you use various object functions of the filter.To customize the sensor names, you must use the

`insOptions`

object.

`ReferenceFrame`

— Reference frame of the extended Kalman filter object, specified as`"NED"`

for the north-east-down frame by default. To customize it as`"ENU"`

for the east-north-up frame, you must use the`insOptions`

object.**Note**You can also specify the data type used in the filter as

`double`

(default) or`single`

by specifying the`DataType`

property of the`insOptions`

object.

### Object Functions

The `insEKF`

object provides various object functions for implementing common
filter workflows, accessing filter states and covariances, and filter tuning.

These object functions support common filter workflows:

`predict`

— Predict the filter state forward in time based on the motion model used in the filter.`fuse`

— Fuse or correct the filter state using a measurement based on a sensor model previously added to the filter. You can use this object function multiple times if you need to fuse multiple measurements.`correct`

— Correct the filter using direct state measurement of the filter state. A direct measurement contains a subset of the filter state vector elements. You can use this object function multiple times for multiple direct measurements.`residual`

— Return the residual and residual covariance of a measurement based on the current state of the filter.`estimateStates`

— Obtain the state estimates based on a timetable of sensor data. The object function processes the measurements one-by-one and returns the corresponding state estimates.

These object functions enable you to access various states and variables maintained by the filter:

`stateinfo`

— Return or display the state components saved in the`State`

property of the filter. Using this function, you can observe what components the state consists of and the indices for all the state components.`stateparts`

— Get or set parts of the state vector. Since the state vector contain many components, this object function enables you to get or set only a component of the whole state vector.`statecovparts`

— Get or set parts of the state estimate error covariance matrix. Similar to the`stateparts`

function, this object function enables you to get or set only a part of the state estimate error covariance matrix.

To obtain more accurate state estimates, you often need to tune the filter parameters
to reduce estimate errors. The `insEKF`

object provides these object
functions to facilitate filter tuning:

`tune`

— Adjust the`AdditiveProcessNoise`

property of the filter object and measurement noise of the sensors to reduce the state estimation error between the filter estimates and the ground truth.You can use the

`tunernoise`

function to obtain an example for the measure noise structure, required by the`tune`

function.You can optionally use the

`tunerconfig`

object to specify tuning parameters, such as function tolerance and the cost function, as well as which elements of the process noise matrix to tuned.

`createTunerCostTemplate`

— Creates a template for a tuner cost function that you can further modify to customize your own cost function.`tunerCostFcnParam`

— Creates a required structure for tuning an`insEKF`

filter with a custom cost function. The structure is useful when generating C-code for a cost function using MATLAB^{®}Coder™.

You can use the `copy`

and `reset`

object functions
to conveniently create a new `insEKF`

object based on an existing
`insEKF`

object.

### Example: Fuse Inertial Sensor Data Using `insEKF`

This example introduces the basic workflows for fusing inertial sensor data using the `insEKF`

object, which supports a flexible fusion framework based on a continuous discrete Kalman filter.

**Create insEKF Object**

Create an `insEKF`

object directly.

filter1 = insEKF

filter1 = insEKF with properties: State: [13×1 double] StateCovariance: [13×13 double] AdditiveProcessNoise: [13×13 double] MotionModel: [1×1 insMotionOrientation] Sensors: {[1×1 insAccelerometer] [1×1 insGyroscope]} SensorNames: {'Accelerometer' 'Gyroscope'} ReferenceFrame: 'NED'

From the `Sensors`

property, note that the object contains two sensor models, `insAccelerometer`

and `insGyroscope`

by default. These enables you to fuse accelerometer and gyroscope data, respectively. From the `MotionModel`

property, note that the object defaults to an `insMotionOrientation`

model, which models rotation-only motion and not translational motion. Due to the specified motion model and sensor models, the state of the filter is a 13-by-1 vector. Get the components and corresponding indices from the state vector using the `stateinfo`

object function.

stateinfo(filter1)

`ans = `*struct with fields:*
Orientation: [1 2 3 4]
AngularVelocity: [5 6 7]
Accelerometer_Bias: [8 9 10]
Gyroscope_Bias: [11 12 13]

Note that, in addition to the orientation and angular velocity states, the filter also includes the accelerometer bias and gyroscope bias.

You can explicitly specify the motion model and sensor models when constructing the filter. For example, create an `insEKF`

object and specify the motion model as an `insMotionPose`

object, which models both rotational motion and translational motion, and specify the sensors as an `insAccelerometer`

, an `insGPS`

, and another `insGPS`

object. This enables the fusion of one set of accelerometer data and two sets of GPS data.

filter2 = insEKF(insAccelerometer,insGPS,insGPS,insMotionPose)

filter2 = insEKF with properties: State: [19×1 double] StateCovariance: [19×19 double] AdditiveProcessNoise: [19×19 double] MotionModel: [1×1 insMotionPose] Sensors: {[1×1 insAccelerometer] [1×1 insGPS] [1×1 insGPS]} SensorNames: {'Accelerometer' 'GPS' 'GPS_1'} ReferenceFrame: 'NED'

The `State`

property is a 19-by-1 vector that contains these components:

stateinfo(filter2)

`ans = `*struct with fields:*
Orientation: [1 2 3 4]
AngularVelocity: [5 6 7]
Position: [8 9 10]
Velocity: [11 12 13]
Acceleration: [14 15 16]
Accelerometer_Bias: [17 18 19]

The `SensorNames`

property enables you to indicate specific sensors when using various object functions of the filter. The filter generates the names for added sensors in a default format. To provide custom names for the sensors, you must use the `insOptions`

object. The `insOptions`

object can also specify the data type of variables used in the filter and the `ReferenceFrame`

of the filter.

options = insOptions(SensorNamesSource="Property", ... SensorNames={'Sensor1','Sensor2','Sensor3'}, ... Datatype="single", ... ReferenceFrame="ENU"); filter3 = insEKF(insAccelerometer,insGPS,insGPS,insMotionPose,options)

filter3 = insEKF with properties: State: [19×1 single] StateCovariance: [19×19 single] AdditiveProcessNoise: [19×19 single] MotionModel: [1×1 insMotionPose] Sensors: {[1×1 insAccelerometer] [1×1 insGPS] [1×1 insGPS]} SensorNames: {'Sensor1' 'Sensor2' 'Sensor3'} ReferenceFrame: 'ENU'

You can also directly obtain the indices of a state component based on the sensor names. For example,

`stateinfo(filter3,'Sensor1_Bias')`

`ans = `*1×3*
17 18 19

**Configure Filter Properties**

Create a new `insEKF`

object with an accelerometer and a gyroscope. Explicitly define these two sensors for later usage.

accSensor = insAccelerometer; gyroSensor = insGyroscope; filter = insEKF(accSensor,gyroSensor)

filter = insEKF with properties: State: [13×1 double] StateCovariance: [13×13 double] AdditiveProcessNoise: [13×13 double] MotionModel: [1×1 insMotionOrientation] Sensors: {[1×1 insAccelerometer] [1×1 insGyroscope]} SensorNames: {'Accelerometer' 'Gyroscope'} ReferenceFrame: 'NED'

Load prerecorded data for an accelerometer and a gyroscope. The sample rate of the recorded data is 100 Hz. It contains the sensor data, ground truth, and the initial orientation represented by a quaternion.

`ld = load("accelGyroINSEKFData.mat")`

`ld = `*struct with fields:*
sampleRate: 100
sensorData: [300×2 timetable]
groundTruth: [300×1 timetable]
initOrient: [1×1 quaternion]

Before fusing the sensor data, you need to set up the initial orientation for the filter state. First, observe the state information.

stateinfo(filter)

`ans = `*struct with fields:*
Orientation: [1 2 3 4]
AngularVelocity: [5 6 7]
Accelerometer_Bias: [8 9 10]
Gyroscope_Bias: [11 12 13]

Query the index of a state component directly by using the corresponding sensor.

`stateinfo(filter,accSensor,"Bias")`

`ans = `*1×3*
8 9 10

Set only the `Orientation`

component of the state vector using the `stateparts`

object function.

quatElements = compact(ld.initOrient); % Convert the quaternion object to a vector of four elements stateparts(filter,"Orientation",quatElements); % Specify the Orientation state component stateparts(filter,"Orientation") % Show the specified Orientation state component

`ans = `*1×4*
1.0000 -0.0003 0.0001 0.0002

Specify the covariance matrix corresponding to the orientation as a diagonal matrix.

`statecovparts(filter,"Orientation",1e-2);`

**Fuse Sensor Data**

You can fuse sensor data by recursively calling the `predict`

and `fuse`

object functions.

Preallocate variables for saving estimated results.

N = size(ld.sensorData,1); estOrient = quaternion.zeros(N,1); dt = seconds(diff(ld.sensorData.Properties.RowTimes));

Predict the filter state, fuse the sensor data, and obtain the estimates.

for ii = 1:N if ii ~= 1 % Predict forward in time. predict(filter,dt(ii-1)); end % Fuse accelerometer data. fuse(filter,accSensor,ld.sensorData.Accelerometer(ii,:),1); % Fuse gyroscope data. fuse(filter,gyroSensor,ld.sensorData.Gyroscope(ii,:),1); % Extract the orientation state estimate using the stateparts object function. estOrient(ii) = quaternion(stateparts(filter,"Orientation")); end

Visualize the estimate error, in quaternion distance, using the `dist`

object function of the `quaternion`

object.

figure times = ld.groundTruth.Properties.RowTimes; distance = rad2deg(dist(estOrient,ld.groundTruth.Orientation)); plot(times,distance) xlabel("Time (sec)") ylabel("Distance (Degrees)") title("Orientation Estimate Error")

The results indicate that the estimate errors are large. You can also use the `estimateStates`

object function to process the sensor data and obtain the same results.

**Tune Filter**

Given that the estimation results are not ideal, you can try to tune the filter parameters to reduce estimate errors.

Create a measurement noise structure and a `tunerconfig`

object used to configure the tuning parameters.

mnoise = tunernoise(filter); cfg = tunerconfig(filter,MaxIterations=10,ObjectiveLimit=1e-4);

Reinitialize the filter. Use the `tune`

object function to tune the filter and obtain the tuned noise.

stateparts(filter,"Orientation",quatElements); statecovparts(filter,"Orientation",1e-2); tunedmn = tune(filter,mnoise,ld.sensorData,ld.groundTruth,cfg);

Iteration Parameter Metric _________ _________ ______ 1 AdditiveProcessNoise(1) 0.3787 1 AdditiveProcessNoise(15) 0.3761 1 AdditiveProcessNoise(29) 0.3695 1 AdditiveProcessNoise(43) 0.3655 1 AdditiveProcessNoise(57) 0.3533 1 AdditiveProcessNoise(71) 0.3446 1 AdditiveProcessNoise(85) 0.3431 1 AdditiveProcessNoise(99) 0.3428 1 AdditiveProcessNoise(113) 0.3427 1 AdditiveProcessNoise(127) 0.3426 1 AdditiveProcessNoise(141) 0.3298 1 AdditiveProcessNoise(155) 0.3206 1 AdditiveProcessNoise(169) 0.3200 1 AccelerometerNoise 0.3199 1 GyroscopeNoise 0.3198 2 AdditiveProcessNoise(1) 0.3126 2 AdditiveProcessNoise(15) 0.3098 2 AdditiveProcessNoise(29) 0.3018 2 AdditiveProcessNoise(43) 0.2988 2 AdditiveProcessNoise(57) 0.2851 2 AdditiveProcessNoise(71) 0.2784 2 AdditiveProcessNoise(85) 0.2760 2 AdditiveProcessNoise(99) 0.2744 2 AdditiveProcessNoise(113) 0.2744 2 AdditiveProcessNoise(127) 0.2743 2 AdditiveProcessNoise(141) 0.2602 2 AdditiveProcessNoise(155) 0.2537 2 AdditiveProcessNoise(169) 0.2527 2 AccelerometerNoise 0.2524 2 GyroscopeNoise 0.2524 3 AdditiveProcessNoise(1) 0.2476 3 AdditiveProcessNoise(15) 0.2432 3 AdditiveProcessNoise(29) 0.2397 3 AdditiveProcessNoise(43) 0.2381 3 AdditiveProcessNoise(57) 0.2255 3 AdditiveProcessNoise(71) 0.2226 3 AdditiveProcessNoise(85) 0.2221 3 AdditiveProcessNoise(99) 0.2202 3 AdditiveProcessNoise(113) 0.2201 3 AdditiveProcessNoise(127) 0.2201 3 AdditiveProcessNoise(141) 0.2090 3 AdditiveProcessNoise(155) 0.2070 3 AdditiveProcessNoise(169) 0.2058 3 AccelerometerNoise 0.2052 3 GyroscopeNoise 0.2052 4 AdditiveProcessNoise(1) 0.2051 4 AdditiveProcessNoise(15) 0.2027 4 AdditiveProcessNoise(29) 0.2019 4 AdditiveProcessNoise(43) 0.2000 4 AdditiveProcessNoise(57) 0.1909 4 AdditiveProcessNoise(71) 0.1897 4 AdditiveProcessNoise(85) 0.1882 4 AdditiveProcessNoise(99) 0.1871 4 AdditiveProcessNoise(113) 0.1870 4 AdditiveProcessNoise(127) 0.1870 4 AdditiveProcessNoise(141) 0.1791 4 AdditiveProcessNoise(155) 0.1783 4 AdditiveProcessNoise(169) 0.1751 4 AccelerometerNoise 0.1748 4 GyroscopeNoise 0.1747 5 AdditiveProcessNoise(1) 0.1742 5 AdditiveProcessNoise(15) 0.1732 5 AdditiveProcessNoise(29) 0.1712 5 AdditiveProcessNoise(43) 0.1712 5 AdditiveProcessNoise(57) 0.1626 5 AdditiveProcessNoise(71) 0.1615 5 AdditiveProcessNoise(85) 0.1598 5 AdditiveProcessNoise(99) 0.1590 5 AdditiveProcessNoise(113) 0.1589 5 AdditiveProcessNoise(127) 0.1589 5 AdditiveProcessNoise(141) 0.1517 5 AdditiveProcessNoise(155) 0.1508 5 AdditiveProcessNoise(169) 0.1476 5 AccelerometerNoise 0.1473 5 GyroscopeNoise 0.1470 6 AdditiveProcessNoise(1) 0.1470 6 AdditiveProcessNoise(15) 0.1470 6 AdditiveProcessNoise(29) 0.1463 6 AdditiveProcessNoise(43) 0.1462 6 AdditiveProcessNoise(57) 0.1367 6 AdditiveProcessNoise(71) 0.1360 6 AdditiveProcessNoise(85) 0.1360 6 AdditiveProcessNoise(99) 0.1350 6 AdditiveProcessNoise(113) 0.1350 6 AdditiveProcessNoise(127) 0.1350 6 AdditiveProcessNoise(141) 0.1289 6 AdditiveProcessNoise(155) 0.1288 6 AdditiveProcessNoise(169) 0.1262 6 AccelerometerNoise 0.1253 6 GyroscopeNoise 0.1246 7 AdditiveProcessNoise(1) 0.1246 7 AdditiveProcessNoise(15) 0.1244 7 AdditiveProcessNoise(29) 0.1205 7 AdditiveProcessNoise(43) 0.1203 7 AdditiveProcessNoise(57) 0.1125 7 AdditiveProcessNoise(71) 0.1122 7 AdditiveProcessNoise(85) 0.1117 7 AdditiveProcessNoise(99) 0.1106 7 AdditiveProcessNoise(113) 0.1104 7 AdditiveProcessNoise(127) 0.1104 7 AdditiveProcessNoise(141) 0.1058 7 AdditiveProcessNoise(155) 0.1052 7 AdditiveProcessNoise(169) 0.1035 7 AccelerometerNoise 0.1024 7 GyroscopeNoise 0.1014 8 AdditiveProcessNoise(1) 0.1014 8 AdditiveProcessNoise(15) 0.1012 8 AdditiveProcessNoise(29) 0.1012 8 AdditiveProcessNoise(43) 0.1005 8 AdditiveProcessNoise(57) 0.0948 8 AdditiveProcessNoise(71) 0.0948 8 AdditiveProcessNoise(85) 0.0938 8 AdditiveProcessNoise(99) 0.0934 8 AdditiveProcessNoise(113) 0.0931 8 AdditiveProcessNoise(127) 0.0931 8 AdditiveProcessNoise(141) 0.0896 8 AdditiveProcessNoise(155) 0.0889 8 AdditiveProcessNoise(169) 0.0867 8 AccelerometerNoise 0.0859 8 GyroscopeNoise 0.0851 9 AdditiveProcessNoise(1) 0.0851 9 AdditiveProcessNoise(15) 0.0850 9 AdditiveProcessNoise(29) 0.0824 9 AdditiveProcessNoise(43) 0.0819 9 AdditiveProcessNoise(57) 0.0771 9 AdditiveProcessNoise(71) 0.0771 9 AdditiveProcessNoise(85) 0.0762 9 AdditiveProcessNoise(99) 0.0759 9 AdditiveProcessNoise(113) 0.0754 9 AdditiveProcessNoise(127) 0.0754 9 AdditiveProcessNoise(141) 0.0734 9 AdditiveProcessNoise(155) 0.0724 9 AdditiveProcessNoise(169) 0.0702 9 AccelerometerNoise 0.0697 9 GyroscopeNoise 0.0689 10 AdditiveProcessNoise(1) 0.0689 10 AdditiveProcessNoise(15) 0.0686 10 AdditiveProcessNoise(29) 0.0658 10 AdditiveProcessNoise(43) 0.0655 10 AdditiveProcessNoise(57) 0.0622 10 AdditiveProcessNoise(71) 0.0620 10 AdditiveProcessNoise(85) 0.0616 10 AdditiveProcessNoise(99) 0.0615 10 AdditiveProcessNoise(113) 0.0607 10 AdditiveProcessNoise(127) 0.0606 10 AdditiveProcessNoise(141) 0.0590 10 AdditiveProcessNoise(155) 0.0578 10 AdditiveProcessNoise(169) 0.0565 10 AccelerometerNoise 0.0562 10 GyroscopeNoise 0.0557

filter.AdditiveProcessNoise

`ans = `*13×13*
0.5849 0 0 0 0 0 0 0 0 0 0 0 0
0 0.6484 0 0 0 0 0 0 0 0 0 0 0
0 0 0.5634 0 0 0 0 0 0 0 0 0 0
0 0 0 1.4271 0 0 0 0 0 0 0 0 0
0 0 0 0 4.3574 0 0 0 0 0 0 0 0
0 0 0 0 0 2.9527 0 0 0 0 0 0 0
0 0 0 0 0 0 1.3071 0 0 0 0 0 0
0 0 0 0 0 0 0 4.3574 0 0 0 0 0
0 0 0 0 0 0 0 0 2.2415 0 0 0 0
0 0 0 0 0 0 0 0 0 0.6449 0 0 0
⋮

Batch-process the sensor data using the `estimateStates`

object function. You can also recursively call the `predict`

and `fuse`

object functions to obtain the same results.

tunedEst = estimateStates(filter,ld.sensorData,tunedmn);

Compare the tuned and untuned estimates against the truth data. The results indicate that the tuning process greatly reduces the estimate errors.

distanceTuned = rad2deg(dist(tunedEst.Orientation,ld.groundTruth.Orientation)); hold on plot(times,distanceTuned) legend("Untuned","Tuned")