imufilter

从加速度计和陀螺仪readi取向ngs

expand all in page

Description

TheimufilterSystem object™ fuses accelerometer and gyroscope sensor data to estimate device orientation.

To estimate device orientation:

Create theimufilterobject and set its properties.
Call the object with arguments, as if it were a function.

学习米ore about how System objects work, seeWhat Are System Objects?

Creation

Syntax

FUSE = imufilter

FUSE = imufilter('ReferenceFrame',RF)

FUSE = imufilter(___,Name,Value)

Description

example

FUSE= imufilterreturns an indirect Kalman filter System object,FUSE,加速度计和陀螺仪数据融合的to estimate device orientation. The filter uses a nine-element state vector to track error in the orientation estimate, the gyroscope bias estimate, and the linear acceleration estimate.

FUSE= imufilter('ReferenceFrame',RF)returns animufilterfilter System object that fuses accelerometer and gyroscope data to estimate device orientation relative to the reference frameRF. SpecifyRFas'NED'(North-East-Down) or'ENU'(East-North-Up). The default value is'NED'.

example

FUSE= imufilter(___,Name,Value)sets each propertyNameto the specifiedValue. Unspecified properties have default values.

Example:FUSE = imufilter('SampleRate',200,'GyroscopeNoise',1e-6)creates a System object,FUSE, with a 200 Hz sample rate and gyroscope noise set to 1e-6 radians per second squared.

Properties

expand all

Unless otherwise indicated, properties arenontunable, which means you cannot change their values after calling the object. Objects lock when you call them, and thereleasefunction unlocks them.

If a property istunable, you can change its value at any time.

For more information on changing property values, seeSystem Design in MATLAB Using System Objects.

`SampleRate`—Sample rate of input sensor data (Hz)
`100`(default) |positive finite scalar

Sample rate of the input sensor data in Hz, specified as a positive finite scalar.

Tunable:No

`DecimationFactor`—Decimation factor
`1`(default) |positive integer scalar

Decimation factor by which to reduce the sample rate of the input sensor data, specified as a positive integer scalar.

The number of rows of the inputs,accelReadingsandgyroReadings, must be a multiple of the decimation factor.

Tunable:No

`AccelerometerNoise`—Variance of accelerometer signal noise ((m/s²)²)
`0.00019247`(default) |positive real scalar

Variance of accelerometer signal noise in (m/s²)², specified as a positive real scalar.

Tunable:Yes

`GyroscopeNoise`—Variance of gyroscope signal noise ((rad/s)²)
`9.1385e-5`(default) |positive real scalar

Variance of gyroscope signal noise in (rad/s)², specified as a positive real scalar.

Tunable:Yes

`GyroscopeDriftNoise`—Variance of gyroscope offset drift ((rad/s)²)
`3.0462e-13`(default) |positive real scalar

Variance of gyroscope offset drift in (rad/s)², specified as a positive real scalar.

Tunable:Yes

`LinearAccelerationNoise`—Variance of linear acceleration noise ((m/s²)²)
`0.0096236`(default) |positive real scalar

Variance of linear acceleration noise in (m/s²)², specified as a positive real scalar. Linear acceleration is modeled as a lowpass filtered white noise process.

Tunable:Yes

`LinearAcclerationDecayFactor`—Decay factor for linear acceleration drift
`0.5`(default) |scalar in the range [0,1]

Decay factor for linear acceleration drift, specified as a scalar in the range [0,1]. If linear acceleration is changing quickly, setLinearAccelerationDecayFactorto a lower value. If linear acceleration changes slowly, setLinearAccelerationDecayFactorto a higher value. Linear acceleration drift is modeled as a lowpass-filtered white noise process.

Tunable:Yes

`InitialProcessNoise`—Covariance matrix for process noise
9-by-9 matrix

Covariance matrix for process noise, specified as a 9-by-9 matrix. The default is:

Columns 1 through 6 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000006092348396 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0.000076154354947 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 Columns 7 through 9 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.009623610000000 0 0 0 0.009623610000000 0 0 0 0.009623610000000

The initial process covariance matrix accounts for the error in the process model.

`OrientationFormat`—Output orientation format
`'quaternion'`(default) |`'Rotation matrix'`

面向输出格式,指定为'quaternion'or'Rotation matrix'. The size of the output depends on the input size,N, and the output orientation format:

'quaternion'–– Output is anN-by-1quaternion.
'Rotation matrix'–– Output is a 3-by-3-by-Nrotation matrix.

Data Types:char|string

Usage

Syntax

(定位、angularVelocity) =保险丝(accelReadings,gyroReadings)

Description

example

[orientation,angularVelocity] = FUSE(accelReadings,gyroReadings)fuses accelerometer and gyroscope readings to compute orientation and angular velocity measurements. The algorithm assumes that the device is stationary before the first call.

Input Arguments

expand all

`accelReadings`—Accelerometer readings in sensor body coordinate system (m/s²)
N-by-3 matrix

Accelerometer readings in the sensor body coordinate system in m/s², specified as anN-by-3 matrix.Nis the number of samples, and the three columns ofaccelReadingsrepresent the [xyz] measurements. Accelerometer readings are assumed to correspond to the sample rate specified by theSampleRateproperty.

Data Types:single|double

`gyroReadings`—Gyroscope readings in sensor body coordinate system (rad/s)
N-by-3 matrix

Gyroscope readings in the sensor body coordinate system in rad/s, specified as anN-by-3 matrix.Nis the number of samples, and the three columns ofgyroReadingsrepresent the [xyz] measurements. Gyroscope readings are assumed to correspond to the sample rate specified by theSampleRateproperty.

Data Types:single|double

Output Arguments

expand all

`orientation`— Orientation that rotates quantities from global coordinate system to sensor body coordinate system
M-by-1 vector of quaternions (default) | 3-by-3-by-Marray

Orientation that can rotate quantities from a global coordinate system to a body coordinate system, returned as quaternions or an array. The size and type oforientationdepends on whether theOrientationFormatproperty is set to'quaternion'or'Rotation matrix':

'quaternion'–– The output is anM-by-1 vector of quaternions, with the same underlying data type as the inputs.
'Rotation matrix'–– The output is a 3-by-3-by-Marray of rotation matrices the same data type as the inputs.

The number of input samples,N, and theDecimationFactorproperty determineM.

You can useorientationin arotateframefunction to rotate quantities from a global coordinate system to a sensor body coordinate system.

Data Types:quaternion|single|double

`angularVelocity`— Angular velocity in sensor body coordinate system (rad/s)
M-by-3 array (default)

Angular velocity with gyroscope bias removed in the sensor body coordinate system in rad/s, returned as anM-by-3 array. The number of input samples,N, and theDecimationFactorproperty determineM.

Data Types:single|double

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object namedobj, use this syntax:

release(obj)

expand all

Specific to`imufilter`

tune Tuneimufilterparameters to reduce estimation error

Common to All System Objects

`step`	RunSystem objectalgorithm
`release`	Release resources and allow changes toSystem objectproperty values and input characteristics
`reset`	Reset internal states ofSystem object

Examples

collapse all

Estimate Orientation from IMU data

Open Live Script

Load therpy_9axisfile, which contains recorded accelerometer, gyroscope, and magnetometer sensor data from a device oscillating in pitch (aroundy-axis), then yaw (aroundz-axis), and then roll (aroundx-axis). The file also contains the sample rate of the recording.

load'rpy_9axis.mat'sensorDataFsaccelerometerReadings = sensorData.Acceleration; gyroscopeReadings = sensorData.AngularVelocity;

Create animufilterSystem object™ with sample rate set to the sample rate of the sensor data. Specify a decimation factor of two to reduce the computational cost of the algorithm.

decim = 2; fuse = imufilter('SampleRate',Fs,'DecimationFactor',decim);

Pass the accelerometer readings and gyroscope readings to theimufilterobject,fuse, to output an estimate of the sensor body orientation over time. By default, the orientation is output as a vector of quaternions.

q = fuse(accelerometerReadings,gyroscopeReadings);

Orientation is defined by the angular displacement required to rotate a parent coordinate system to a child coordinate system. Plot the orientation in Euler angles in degrees over time.

imufilterfusion correctly estimates the change in orientation from an assumed north-facing initial orientation. However, the device'sx-axis was pointing southward when recorded. To correctly estimate the orientation relative to the true initial orientation or relative to NED, useahrsfilter.

time = (0:decim:size(accelerometerReadings,1)-1)/Fs; plot(time,eulerd(q,'ZYX','frame')) title('Orientation Estimate') legend('Z-axis','Y-axis','X-axis') xlabel('Time (s)') ylabel('Rotation (degrees)')

Figure contains an axes object. The axes object with title Orientation Estimate contains 3 objects of type line. These objects represent Z-axis, Y-axis, X-axis.

Model Tilt Using Gyroscope and Accelerometer Readings

Open Script

Model a tilting IMU that contains an accelerometer and gyroscope using theimuSensorSystem object™. Use ideal and realistic models to compare the results of orientation tracking using theimufilterSystem object.

Load a struct describing ground-truth motion and a sample rate. The motion struct describes sequential rotations:

yaw: 120 degrees over two seconds
pitch: 60 degrees over one second
roll: 30 degrees over one-half second
roll: -30 degrees over one-half second
pitch: -60 degrees over one second
yaw: -120 degrees over two seconds

In the last stage, the motion struct combines the 1st, 2nd, and 3rd rotations into a single-axis rotation. The acceleration, angular velocity, and orientation are defined in the local NED coordinate system.

loady120p60r30.matmotionfsaccNED = motion.Acceleration; angVelNED = motion.AngularVelocity; orientationNED = motion.Orientation; numSamples = size(motion.Orientation,1); t = (0:(numSamples-1)).'/fs;

Create an ideal IMU sensor object and a default IMU filter object.

IMU = imuSensor('accel-gyro','SampleRate',fs); aFilter = imufilter('SampleRate',fs);

In a loop:

Simulate IMU output by feeding the ground-truth motion to the IMU sensor object.
Filter the IMU output using the default IMU filter object.

orientation = zeros(numSamples,1,'quaternion');fori = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:)); orientation(i) = aFilter(accelBody,gyroBody);endrelease(aFilter)

Plot the orientation over time.

figure(1) plot(t,eulerd(orientation,'ZYX','frame')) xlabel('Time (s)') ylabel('Rotation (degrees)') title('Orientation Estimation -- Ideal IMU Data, Default IMU Filter') legend('Z-axis','Y-axis','X-axis')

Modify properties of yourimuSensorto model real-world sensors. Run the loop again and plot the orientation estimate over time.

IMU.Accelerometer = accelparams(...'MeasurementRange',19.62,...'Resolution',0.00059875,...'ConstantBias',0.4905,...'AxesMisalignment',2,...'NoiseDensity',0.003924,...'BiasInstability',0,...'TemperatureBias', [0.34335 0.34335 0.5886],...'TemperatureScaleFactor',0.02); IMU.Gyroscope = gyroparams(...'MeasurementRange',4.3633,...'Resolution',0.00013323,...'AxesMisalignment',2,...'NoiseDensity',8.7266e-05,...'TemperatureBias',0.34907,...'TemperatureScaleFactor',0.02,...'AccelerationBias',0.00017809,...'ConstantBias',[0.3491,0.5,0]); orientationDefault = zeros(numSamples,1,'quaternion');fori = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:)); orientationDefault(i) = aFilter(accelBody,gyroBody);endrelease(aFilter) figure(2) plot(t,eulerd(orientationDefault,'ZYX','frame')) xlabel('Time (s)') ylabel('Rotation (degrees)') title('Orientation Estimation -- Realistic IMU Data, Default IMU Filter') legend('Z-axis','Y-axis','X-axis')

The ability of theimufilterto track the ground-truth data is significantly reduced when modeling a realistic IMU. To improve performance, modify properties of yourimufilterobject. These values were determined empirically. Run the loop again and plot the orientation estimate over time.

aFilter.GyroscopeNoise = 7.6154e-7; aFilter.AccelerometerNoise = 0.0015398; aFilter.GyroscopeDriftNoise = 3.0462e-12; aFilter.LinearAccelerationNoise = 0.00096236; aFilter.InitialProcessNoise = aFilter.InitialProcessNoise*10; orientationNondefault = zeros(numSamples,1,'quaternion');fori = 1:numSamples [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:)); orientationNondefault(i) = aFilter(accelBody,gyroBody);endrelease(aFilter) figure(3) plot(t,eulerd(orientationNondefault,'ZYX','frame')) xlabel('Time (s)') ylabel('Rotation (degrees)') title('Orientation Estimation -- Realistic IMU Data, Nondefault IMU Filter') legend('Z-axis','Y-axis','X-axis')

To quantify the improved performance of the modifiedimufilter, plot the quaternion distance between the ground-truth motion and the orientation as returned by theimufilterwith default and nondefault properties.

qDistDefault = rad2deg(dist(orientationNED,orientationDefault)); qDistNondefault = rad2deg(dist(orientationNED,orientationNondefault)); figure(4) plot(t,[qDistDefault,qDistNondefault]) title('Quaternion Distance from True Orientation') legend('Realistic IMU Data, Default IMU Filter',...'Realistic IMU Data, Nondefault IMU Filter') xlabel('Time (s)') ylabel('Quaternion Distance (degrees)')

Remove Bias from Angular Velocity Measurement

Open Live Script

This example shows how to remove gyroscope bias from an IMU usingimufilter.

UsekinematicTrajectoryto create a trajectory with two parts. The first part has a constant angular velocity about they- andz-axes. The second part has a varying angular velocity in all three axes.

duration = 60*8; fs = 20; numSamples = duration * fs; rng('default')% Seed the RNG to reproduce noisy sensor measurements.initialAngVel = [0,0.5,0.25]; finalAngVel = [-0.2,0.6,0.5]; constantAngVel = repmat(initialAngVel,floor(numSamples/2),1); varyingAngVel = [linspace(initialAngVel(1), finalAngVel(1), ceil(numSamples/2)).',...linspace(initialAngVel(2), finalAngVel(2), ceil(numSamples/2)).',...linspace(initialAngVel(3), finalAngVel(3), ceil(numSamples/2)).']; angVelBody = [constantAngVel; varyingAngVel]; accBody = zeros(numSamples,3); traj = kinematicTrajectory('SampleRate',fs); [~,qNED,~,accNED,angVelNED] = traj(accBody,angVelBody);

Create animuSensorSystem object™,IMU, with a nonideal gyroscope. CallIMUwith the ground-truth acceleration, angular velocity, and orientation.

IMU = imuSensor('accel-gyro',...'Gyroscope',gyroparams('RandomWalk',0.003,'ConstantBias',0.3),...'SampleRate',fs); [accelReadings, gyroReadingsBody] = IMU(accNED,angVelNED,qNED);

Create animufilterSystem object,fuse. Callfusewith the modeled accelerometer readings and gyroscope readings.

fuse = imufilter('SampleRate',fs,'GyroscopeDriftNoise', 1e-6); [~,angVelBodyRecovered] = fuse(accelReadings,gyroReadingsBody);

Plot the ground-truth angular velocity, the gyroscope readings, and the recovered angular velocity for each axis.

The angular velocity returned from theimufiltercompensates for the effect of the gyroscope bias over time and converges to the true angular velocity.

time = (0:numSamples-1)'/fs; figure(1) plot(time,angVelBody(:,1),...time,gyroReadingsBody(:,1),...time,angVelBodyRecovered(:,1)) title('X-axis') legend('True Angular Velocity',...'Gyroscope Readings',...'Recovered Angular Velocity') ylabel('Angular Velocity (rad/s)')

Figure contains an axes object. The axes object with title X-axis contains 3 objects of type line. These objects represent True Angular Velocity, Gyroscope Readings, Recovered Angular Velocity.

figure(2) plot(time,angVelBody(:,2),...time,gyroReadingsBody(:,2),...time,angVelBodyRecovered(:,2)) title('Y-axis') ylabel('Angular Velocity (rad/s)')

Figure contains an axes object. The axes object with title Y-axis contains 3 objects of type line.

figure(3) plot(time,angVelBody(:,3),...time,gyroReadingsBody(:,3),...time,angVelBodyRecovered(:,3)) title('Z-axis') ylabel('Angular Velocity (rad/s)') xlabel('Time (s)')

Figure contains an axes object. The axes object with title Z-axis contains 3 objects of type line.

Algorithms

expand all

Note: The following algorithm only applies to an NED reference frame.

Theimufilteruses the six-axis Kalman filter structure described in[1]. The algorithm attempts to track the errors in orientation, gyroscope offset, and linear acceleration to output the final orientation and angular velocity. Instead of tracking the orientation directly, the indirect Kalman filter models the error process,x, with a recursive update:

$x_{k} = [\begin{matrix} θ_{k} \\ b_{k} \\ a_{k} \end{matrix}] = F_{k} [\begin{matrix} θ_{k - 1} \\ b_{k - 1} \\ a_{k - 1} \end{matrix}] + w_{k}$

wherex_kis a 9-by-1 vector consisting of:

θ_k–– 3-by-1 orientation error vector, in degrees, at timek
b_k–– 3-by-1 gyroscope zero angular rate bias vector, in deg/s, at timek
a_k–– 3-by-1 acceleration error vector measured in the sensor frame, in g, at timek
w_k–– 9-by-1 additive noise vector
F_k–– state transition model

Becausex_kis defined as the error process, thea prioriestimate is always zero, and therefore the state transition model,F_k, is zero. This insight results in the following reduction of the standard Kalman equations:

Standard Kalman equations:

$\begin{array}{l} x_{k}^{-} = F_{k} x_{k - 1}^{+} \\ P_{k}^{-} = F_{k} P_{k - 1}^{+} F_{k}^{T} + Q_{k} \\ y_{k} = z_{k} - H_{k} x_{k}^{-} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = x_{k}^{-} + K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

Kalman equations used in this algorithm:

$\begin{array}{l} x_{k}^{-} = 0 \\ P_{k}^{-} = Q_{k} \\ y_{k} = z_{k} \\ S_{k} = R_{k} + H_{k} P_{k}^{-} H_{k}^{T} \\ K_{k} = P_{k}^{-} H_{k}^{T} {(S_{k})}^{- 1} \\ x_{k}^{+} = K_{k} y_{k} \\ P_{k}^{+} = P_{k}^{-} - K_{k} H_{k} P_{k}^{-} \end{array}$

where

x_k⁻–– predicted (a priori) state estimate; the error process
P_k⁻–– predicted (a priori) estimate covariance
y_k–– innovation
S_k–– innovation covariance
K_k–– Kalman gain
x_k⁺–– updated (a posteriori) state estimate
P_k⁺–– updated (a posteriori) estimate covariance

krepresents the iteration, the superscript⁺represents ana posterioriestimate, and the superscript⁻represents ana prioriestimate.

The graphic and following steps describe a single frame-based iteration through the algorithm.

Algorithm Flowchart

Before the first iteration, theaccelReadingsandgyroReadingsinputs are chunked into 1-by-3 frames andDecimationFactor-by-3 frames, respectively. The algorithm uses the most current accelerometer readings corresponding to the chunk of gyroscope readings.

Detailed Overview

Step through the algorithm for an explanation of each stage of the detailed overview.

Model

The algorithm models acceleration and angular change as linear processes.

Predict Orientation

The orientation for the current frame is predicted by first estimating the angular change from the previous frame:

$Δ φ_{N \times 3} = \frac{(g y r o R e a d i n g s_{N \times 3} - g y r o O f f s e t_{1 \times 3})}{f s}$

whereN是指定的大量毁灭的因素DecimationFactorproperty, andfsis the sample rate specified by theSampleRateproperty.

The angular change is converted into quaternions using therotvecquaternionconstruction syntax:

$Δ Q_{N \times 1} = quaternion (Δ φ_{N \times 3},' r o t v e c')$

The previous orientation estimate is updated by rotating it by ΔQ:

$q_{1 \times 1}^{-} = (q_{1 \times 1}^{+}) (\prod_{n = 1}^{N} Δ Q_{n})$

During the first iteration, the orientation estimate,q⁻, is initialized byecompasswith an assumption that thex-axis points north.

Estimate Gravity from Orientation

The gravity vector is interpreted as the third column of the quaternion,q⁻, in rotation matrix form:

$g_{1 \times 3} = {(r P r i o r (:, 3))}^{T}$

Seeecompassfor an explanation of why the third column ofrPriorcan be interpreted as the gravity vector.

Estimate Gravity from Acceleration

A second gravity vector estimation is made by subtracting the decayed linear acceleration estimate of the previous iteration from the accelerometer readings:

$g A c c e l_{1 \times 3} = a c c e l R e a d i n g s_{1 \times 3} - l i n A c c e l p r i o r_{1 \times 3}$

Error Model

The error model is the difference between the gravity estimate from the accelerometer readings and the gravity estimate from the gyroscope readings: $z = g - g A c c e l$ .

Kalman Equations

The Kalman equations use the gravity estimate derived from the gyroscope readings,g, and the observation of the error process,z, to update the Kalman gain and intermediary covariance matrices. The Kalman gain is applied to the error signal,z, to output ana posteriorierror estimate,x⁺.

Observation Model

The observation model maps the 1-by-3 observed state,g, into the 3-by-9 true state,H.

The observation model is constructed as:

$H_{3 \times 9} = [\begin{matrix} 0 & g_{z} & - g_{y} & 0 & - κ g_{z} & κ g_{y} & 1 & 0 & 0 \\ - g_{z} & 0 & g_{x} & κ g_{z} & 0 & - κ g_{x} & 0 & 1 & 0 \\ g_{y} & - g_{x} & 0 & - κ g_{y} & κ g_{x} & 0 & 0 & 0 & 1 \end{matrix}]$

whereg_x,g_y, andg_zare thex-,y-, andz-elements of the gravity vector estimated from the orientation, respectively.κis a constant determined by theSampleRateandDecimationFactorproperties:κ=DecimationFactor/SampleRate.

See sections 7.3 and 7.4 of[1]for a derivation of the observation model.

Innovation Covariance

The innovation covariance is a 3-by-3 matrix used to track the variability in the measurements. The innovation covariance matrix is calculated as:

$S_{3 x 3} = R_{3 x 3} + (H_{3 x 9}) (P_{_{9 x 9}}^{-}) {(H_{3 x 9})}^{T}$

where

His the observation model matrix
P⁻is the predicted (a priori) estimate of the covariance of the observation model calculated in the previous iteration
Ris the covariance of the observation model noise, calculated as:

$R_{3 \times 3} = (λ + ξ + κ (β + η)) [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}] .$

The following properties define the observation model noise variance:
- κ–– (DecimationFactor/SampleRate)²
- β––GyroscopeDriftNoise
- η––GyroscopeNoise
- λ––AccelerometerNoise
- ξ––LinearAccelerationNoise

Update Error Estimate Covariance

The error estimate covariance is a 9-by-9 matrix used to track the variability in the state.

The error estimate covariance matrix is updated as:

$P_{_{9 \times 9}}^{+} = P_{_{9 \times 9}}^{-} - (K_{9 \times 3}) (H_{3 \times 9}) (P_{_{9 \times 9}}^{-})$

whereKis the Kalman gain,His the measurement matrix, andP⁻is the error estimate covariance calculated during the previous iteration.

Predict Error Estimate Covariance

The error estimate covariance is a 9-by-9 matrix used to track the variability in the state. Thea priorierror estimate covariance,P⁻, is set to the process noise covariance,Q, determined during the previous iteration.Qis calculated as a function of thea posteriorierror estimate covariance,P⁺. When calculatingQ, the cross-correlation terms are assumed to be negligible compared to the autocorrelation terms, and are set to zero:

$Q = [\begin{matrix} P^{+} (1) + κ^{2} (P^{+} (31) + β + η) & 0 & 0 & - κ (P^{+} (31) + β) & 0 & 0 & 0 & 0 & 0 \\ 0 & P^{+} (11) + κ^{2} (P^{+} (41) + β + η) & 0 & 0 & - κ (P^{+} (41) + β) & 0 & 0 & 0 & 0 \\ 0 & 0 & P^{+} (21) + κ^{2} (P^{+} (51) + β + η) & 0 & 0 & - κ (P^{+} (51) + β) & 0 & 0 & 0 \\ - κ (P^{+} (31) + β) & 0 & 0 & P^{+} (31) + β & 0 & 0 & 0 & 0 & 0 \\ 0 & - κ (P^{+} (41) + β) & 0 & 0 & P^{+} (41) + β & 0 & 0 & 0 & 0 \\ 0 & 0 & - κ (P^{+} (51) + β) & 0 & 0 & P^{+} (51) + β & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (61) + ξ & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (71) + ξ & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & ν^{2} P^{+} (81) + ξ \end{matrix}]$

where

P⁺–– is the updated (a posteriori) error estimate covariance
κ––DecimationFactor/SampleRate
β––GyroscopeDriftNoise
η––GyroscopeNoise
ν––LinearAcclerationDecayFactor
ξ––LinearAccelerationNoise

See section 10.1 of[1]for a derivation of the terms of the process error matrix.

Kalman Gain

The Kalman gain matrix is a 9-by-3 matrix used to weight the innovation. In this algorithm, the innovation is interpreted as the error process,z.

The Kalman gain matrix is constructed as:

$K_{9 \times 3} = (P_{_{9 \times 9}}^{-}) {(H_{3 \times 9})}^{T} {({(S_{3 \times 3})}^{T})}^{- 1}$

where

P^-–– predicted error covariance
H–– observation model
S–– innovation covariance

Update a Posteriori Error

Thea posteriorerror estimate is determined by combining the Kalman gain matrix with the error in the gravity vector estimations:

$x_{9 \times 1} = (K_{9 \times 3}) {(z_{1 \times 3})}^{T}$

Correct

Estimate Orientation

The orientation estimate is updated by multiplying the previous estimation by the error:

$q^{+} = (q^{-}) (θ^{+})$

Estimate Linear Acceleration

The linear acceleration estimation is updated by decaying the linear acceleration estimation from the previous iteration and subtracting the error:

$l i n A c c e l P r i o r = (l i n A c c e l P r i o r_{k - 1}) ν - a^{+}$

where

ν––LinearAcclerationDecayFactor

Estimate Gyroscope Offset

The gyroscope offset estimation is updated by subtracting the gyroscope offset error from the gyroscope offset from the previous iteration:

$g y r o O f f s e t = g y r o O f f s e t_{k - 1} - b^{+}$

Compute Angular Velocity

To estimate angular velocity, the frame ofgyroReadingsare averaged and the gyroscope offset computed in the previous iteration is subtracted:

$a n g u l a r V e l o c i t y_{1 \times 3} = \frac{\sum g y r o R e a d i n g s_{N \times 3}}{N} - g y r o O f f s e t_{1 \times 3}$

whereN是指定的大量毁灭的因素DecimationFactorproperty.

The gyroscope offset estimation is initialized to zeros for the first iteration.

References

[1] Open Source Sensor Fusion.https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/tree/master/docs

[2] Roetenberg, D., H.J. Luinge, C.T.M. Baten, and P.H. Veltink. "Compensation of Magnetic Disturbances Improves Inertial and Magnetic Sensing of Human Body Segment Orientation."IEEE Transactions on Neural Systems and Rehabilitation Engineering. Vol. 13. Issue 3, 2005, pp. 395-405.

Extended Capabilities

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

Usage notes and limitations:

SeeSystem Objects in MATLAB Code Generation(MATLAB Coder).

Version History

Introduced in R2018b

imufilter

Description

Creation

Syntax

Description

Properties

SampleRate—Sample rate of input sensor data (Hz)100(default) |positive finite scalar

DecimationFactor—Decimation factor1(default) |positive integer scalar

AccelerometerNoise—Variance of accelerometer signal noise ((m/s2)2)0.00019247(default) |positive real scalar

GyroscopeNoise—Variance of gyroscope signal noise ((rad/s)2)9.1385e-5(default) |positive real scalar

GyroscopeDriftNoise—Variance of gyroscope offset drift ((rad/s)2)3.0462e-13(default) |positive real scalar

LinearAccelerationNoise—Variance of linear acceleration noise ((m/s2)2)0.0096236(default) |positive real scalar

LinearAcclerationDecayFactor—Decay factor for linear acceleration drift0.5(default) |scalar in the range [0,1]

InitialProcessNoise—Covariance matrix for process noise9-by-9 matrix

OrientationFormat—Output orientation format'quaternion'(default) |'Rotation matrix'

Usage

Syntax

Description

Input Arguments

accelReadings—Accelerometer readings in sensor body coordinate system (m/s2)N-by-3 matrix

gyroReadings—Gyroscope readings in sensor body coordinate system (rad/s)N-by-3 matrix

Output Arguments

orientation— Orientation that rotates quantities from global coordinate system to sensor body coordinate systemM-by-1 vector of quaternions (default) | 3-by-3-by-Marray

angularVelocity— Angular velocity in sensor body coordinate system (rad/s)M-by-3 array (default)

Object Functions

Specific toimufilter

Common to All System Objects

Examples

Estimate Orientation from IMU data

Model Tilt Using Gyroscope and Accelerometer Readings

Remove Bias from Angular Velocity Measurement

Algorithms

Detailed Overview

Model

Error Model

Kalman Equations

Correct

Compute Angular Velocity

References

Extended Capabilities

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

Version History

See Also

Topics

`SampleRate`—Sample rate of input sensor data (Hz)
`100`(default) |positive finite scalar

`DecimationFactor`—Decimation factor
`1`(default) |positive integer scalar

`AccelerometerNoise`—Variance of accelerometer signal noise ((m/s²)²)
`0.00019247`(default) |positive real scalar

`GyroscopeNoise`—Variance of gyroscope signal noise ((rad/s)²)
`9.1385e-5`(default) |positive real scalar

`GyroscopeDriftNoise`—Variance of gyroscope offset drift ((rad/s)²)
`3.0462e-13`(default) |positive real scalar

`LinearAccelerationNoise`—Variance of linear acceleration noise ((m/s²)²)
`0.0096236`(default) |positive real scalar

`LinearAcclerationDecayFactor`—Decay factor for linear acceleration drift
`0.5`(default) |scalar in the range [0,1]

`InitialProcessNoise`—Covariance matrix for process noise
9-by-9 matrix

`OrientationFormat`—Output orientation format
`'quaternion'`(default) |`'Rotation matrix'`

`accelReadings`—Accelerometer readings in sensor body coordinate system (m/s²)
N-by-3 matrix

`gyroReadings`—Gyroscope readings in sensor body coordinate system (rad/s)
N-by-3 matrix

`orientation`— Orientation that rotates quantities from global coordinate system to sensor body coordinate system
M-by-1 vector of quaternions (default) | 3-by-3-by-Marray

`angularVelocity`— Angular velocity in sensor body coordinate system (rad/s)
M-by-3 array (default)

Specific to`imufilter`

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