Matlabsimulink Sensor Fusion and Tracking Toolbox Reference The Mathworks Instant Download
Matlabsimulink Sensor Fusion and Tracking Toolbox Reference The Mathworks Instant Download
https://ebookbell.com/product/matlabsimulink-sensor-fusion-and-
tracking-toolbox-reference-the-mathworks-36497084
https://ebookbell.com/product/matlabsimulink-radar-toolbox-users-
guide-v13-the-mathworks-46231538
https://ebookbell.com/product/matlabsimulink-vehicle-dynamics-
blockset-users-guide-the-mathworks-46470278
https://ebookbell.com/product/matlab-simulink-ultra-basics-fujii-
toshinori-46694204
https://ebookbell.com/product/matlab-simulink-installation-guide-for-
windows-release-14-product-family-the-mathworks-5429100
Matlab Simulink Documentation Mathworks
https://ebookbell.com/product/matlab-simulink-documentation-
mathworks-6741142
https://ebookbell.com/product/feedback-control-systems-matlabsimulink-
approach-farzin-asadi-11052902
https://ebookbell.com/product/modeling-and-simulation-using-matlab-
simulink-for-ece-dr-shailendra-jain-231663150
https://ebookbell.com/product/state-feedback-control-and-kalman-
filtering-with-matlabsimulink-tutorials-liuping-wang-46482740
https://ebookbell.com/product/disturbance-observer-for-advanced-
motion-control-with-matlab-simulink-akira-shimada-49607168
Sensor Fusion and Tracking Toolbox™
Reference
R2021b
How to Contact MathWorks
Phone: 508-647-7000
Functions
1
Classes
2
System Objects
3
Blocks
4
Apps
5
iii
1
Functions
1 Functions
allanvar
Allan variance
Syntax
[avar,tau] = allanvar(Omega)
[avar,tau] = allanvar(Omega,m)
[avar,tau] = allanvar(Omega,ptStr)
[avar,tau] = allanvar(___,fs)
Description
Allan variance is used to measure the frequency stability of oscillation for a sequence of data in the
time domain. It can also be used to determine the intrinsic noise in a system as a function of the
averaging time. The averaging time series τ can be specified as τ = m/fs. Here fs is the sampling
frequency of data, and m is a list of ascending averaging factors (such as 1, 2, 4, 8, …).
[avar,tau] = allanvar(Omega) returns the Allan variance avar as a function of averaging time
tau. The default averaging time tau is an octave sequence given as (1, 2, ..., 2floor{log2[(N-1)/2]}), where
N is the number of samples in Omega. If Omega is specified as a matrix, allanvar operates over the
columns of omega.
[avar,tau] = allanvar(Omega,m) returns the Allan variance avar for specific values of tau
defined by m. Since the default frequency fs is assumed to be 1, the output tau is exactly same with
m.
[avar,tau] = allanvar(___,fs) also allows you to provide the sampling frequency fs of the
input data omega in Hz. This input parameter can be used with any of the previous syntaxes.
Examples
Load gyroscope data from a MAT file, including the sample rate of the data in Hz. Calculate the Allan
variance.
load('LoggedSingleAxisGyroscope','omega','Fs')
[avar,tau] = allanvar(omega,'octave',Fs);
1-2
allanvar
Generate sample gyroscope noise, including angle random walk and rate random walk.
numSamples = 1e6;
Fs = 100;
nStd = 1e-3;
kStd = 1e-7;
nNoise = nStd.*randn(numSamples,1);
kNoise = kStd.*cumsum(randn(numSamples,1));
omega = nNoise+kNoise;
Calculate the Allan deviation at specific values of m = τ. The Allan deviation is the square root of the
Allan variance.
m = 2.^(9:18);
[avar,tau] = allanvar(omega,m,Fs);
adev = sqrt(avar);
loglog(tau,adev)
xlabel('\tau')
ylabel('\sigma(\tau)')
1-3
1 Functions
title('Allan Deviation')
grid on
Input Arguments
Omega — Input data
N-by-1 vector | N-by-M matrix
Input data specified as an N-by-1 vector or an N-by-M matrix. N is the number of samples, and M is
the number of sample sets. If specified as a matrix, allanvar operates over the columns of Omega.
Data Types: single | double
m — Averaging factor
scalar | vector
Averaging factor, specified as a scalar or vector with ascending integer values less than (N-1)/2,
where N is the number of samples in Omega.
Data Types: single | double
1-4
allanvar
Basic frequency of the input data, Omega, in Hz, specified as a positive scalar.
Data Types: single | double
Output Arguments
avar — Allan variance of input data
vector | matrix
See Also
gyroparams | imuSensor
Introduced in R2019a
1-5
1 Functions
ctrect
Constant turn-rate rectangular target motion model
Syntax
updatedstates = ctrect(states)
updatedstates = ctrect(states,dt)
updatedstates = ctrect(states,w,dt)
Description
updatedstates = ctrect(states) returns the updated rectangular states from the current
rectangular states based on the rectangular target motion model. The default time step is 1 second.
Examples
state = 1×7
state = 1×7
state = 1×7
1-6
ctrect
states = 7×3
states = 7×3
states = 7×3
Input Arguments
states — Current rectangular states
1-by-7 real-valued vector | 7-by-1 real-valued vector | 7-by-N real-valued matrix
1-7
1 Functions
Current rectangular states, specified as a 1-by-7 real-valued vector, 7-by-1 real-valued vector, or a 7-
by-N real-valued matrix, where N is the number of states. The seven dimensional rectangular target
state is defined as [x; y; s; θ; ω; L; W]:
Example: [1;2;2;30;1;4.7;1.8]
Data Types: single | double
dt — Time step
real-valued positive scalar
w — Process noise
real scalar | 2-by-N real-valued matrix
Process noise, specified as a 2-by-N real-valued matrix, where N is the number of states specified in
the states input. If specified as a scalar, it is expanded to a 2-by-N matrix with all elements equal to
1-8
ctrect
the scalar. The first row of the matrix specifies the process noise in acceleration (m/s2). The second
row specifies the process noise in yaw acceleration (degrees/s2).
Data Types: single | double
Output Arguments
updatedstates — Updated states
1-by-7 real-valued vector | 7-by-1 real-valued vector | 7-by-N real-valued matrix
Updated states, specified as a 1-by-7 real-valued vector, a 7-by-1 real-valued vector, or a 7-by-N real-
valued matrix, where N is the number of states. The dimensions and setups of updatedstates
output are exactly the same as those of the states input.
Data Types: single | double
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
See Also
gmphd | trackerPHD | ctrectmeas | ctrectmeasjac | ctrectjac | initctrectgmphd |
ctrectcorners
Introduced in R2019b
1-9
1 Functions
ctrectmeas
Constant turn-rate rectangular target measurement model
Syntax
measurements = ctrectmeas(states,detections)
Description
measurements = ctrectmeas(states,detections) returns the expected measurements from
the current rectangular states and detections.
Examples
load('rectangularTargetDetections.mat','detections','truthState');
Generate expected detections from the target's rectangular state and actual detections using
ctrectmeas.
tgtState = [3;48;0;60;0;5;1.9];
zExp = ctrectmeas(tgtState,detections);
theaterP = theaterPlot;
stateP = trackPlotter(theaterP,'DisplayName','State','MarkerFaceColor','g');
truthP = trackPlotter(theaterP,'DisplayName','Truth','MarkerFaceColor', 'b');
detP = detectionPlotter(theaterP,'DisplayName','Detections','MarkerFaceColor','r');
expDetP = detectionPlotter(theaterP,'DisplayName','Expected Detections','MarkerFaceColor','y');
l = legend(theaterP.Parent);
l.AutoUpdate = 'on';
hold on;
assignP = plot(theaterP.Parent,NaN,NaN,'-.','DisplayName','Association');
inDets = [detections{:}];
inMeas = horzcat(inDets.Measurement);
detP.plotDetection(inMeas');
zExpPlot = reshape(zExp,3,[]);
expDetP.plotDetection(zExpPlot');
zLines = nan(2,numel(detections)*3);
zLines(1,1:3:end) = zExpPlot(1,:);
1-10
ctrectmeas
zLines(2,1:3:end) = zExpPlot(2,:);
zLines(1,2:3:end) = inMeas(1,:);
zLines(2,2:3:end) = inMeas(2,:);
assignP.XData = zLines(1,:);
assignP.YData = zLines(2,:);
truthPos = [truthState(1:2);0];
truthDims = struct('Length',truthState(6),...
'Width',truthState(7),...
'Height', 0,...
'OriginOffset', [0 0 0]);
truthOrient = quaternion([truthState(4) 0 0],'eulerd', 'ZYX','frame');
truthP.plotTrack(truthPos',truthDims,truthOrient);
statePos = [tgtState(1:2);0];
stateDims = struct('Length',tgtState(6),...
'Width',tgtState(7),...
'Height',0,...
'OriginOffset', [0 0 0]);
stateOrient = quaternion([tgtState(4) 0 0],'eulerd', 'ZYX','frame');
stateP.plotTrack(statePos', stateDims, stateOrient);
1-11
1 Functions
Input Arguments
states — Current rectangular states
7-by-N real-valued matrix
Current rectangular states, specified as a 7-by-N real-valued matrix, where N is the number of states.
The seven-dimensional rectangular target state is defined as [x; y; s; θ; ω; L; W]:
Example: [1;2;2;30;1;4.7;1.8]
Data Types: single | double
1-12
ctrectmeas
Output Arguments
measurements — Expected measurements
P-by-N-by-M real-valued array
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
See Also
trackerPHD | gmphd | ctrect | ctrectmeasjac | ctrectjac | initctrectgmphd |
ctrectcorners
Introduced in R2019b
1-13
1 Functions
ctrectmeasjac
Jacobian of constant turn-rate rectangular target measurement model
Syntax
jacobian = ctrectmeasjac(state,detections)
Description
jacobian = ctrectmeasjac(state,detections) returns the Jacobian based on the current
rectangular target state and detections.
Examples
load('rectangularTargetDetections.mat','detections');
Calculate Jacobian based on the rectangular state of the target and detections.
tgtState = [3;48;0;60;0;5;1.9];
jac = ctrectmeasjac(tgtState,detections);
jac1 = jac(:,:,1)
jac1 = 3×7
Input Arguments
state — Current rectangular target state
7-by-1 real-valued vector
Current rectangular target state, specified as a 7-by-1 real-valued vector. The seven dimensional
rectangular target state is defined as [x; y; s; θ; ω; L; W]. The meaning of these variables and their
units are:
1-14
ctrectmeasjac
Example: [1;2;2;30;1;4.7;1.8]
Data Types: single | double
Output Arguments
jacobian — Jacobian of measurement model
M-by-7-by-D real-valued array
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
1-15
1 Functions
See Also
trackerPHD | gmphd | ctrect | ctrectmeas | ctrectjac | initctrectgmphd | ctrectcorners
Introduced in R2019b
1-16
ctrectjac
ctrectjac
Jacobian of constant turn-rate rectangular target motion model
Syntax
Jx = ctrectjac(state)
Jx = ctrectjac(state,dt)
[Jx,Jw] = ctrectjac(state,w,dt)
Description
Jx = ctrectjac(state) returns the Jacobian matrix of the constant turn-rate rectangular motion
model with respect to the state vector. The default time step is 1 second.
Examples
state = [1;2;2;30;1;4.7;1.8];
jac = ctrectjac(state)
jac = 7×7
jac = ctrectjac(state,0.1)
jac = 7×7
1-17
1 Functions
0 0 0 0 1.0000 0 0
0 0 0 0 0 1.0000 0
0 0 0 0 0 0 1.0000
jac = ctrectjac(state,0.01,0.1)
jac = 7×7
Input Arguments
state — Current state
1-by-7 real-valued vector
Current state, specified as a 1-by-7 real-valued vector. The sate of the constant-turn rectangular
target model is [x; y; s; θ; ω; L; W]. The meaning of these variables and their units are:
1-18
ctrectjac
Example: [1;2;2;30;1;4.7;1.8]
Data Types: single | double
dt — Time step
real-valued positive scalar
w — Process noise
real scalar | 2-element real-valued vector
Process noise, specified as a 2-element real-valued vector. The first element specifies the process
noise in linear acceleration (m/s2). The second element specifies the process noise in yaw acceleration
(degrees/s2).
Data Types: single | double
Output Arguments
Jx — Jacobian matrix with respect to state
7-by-7 matrix
1-19
1 Functions
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
See Also
gmphd | trackerPHD | ctrect | ctrectmeas | ctrectmeasjac | initctrectgmphd |
ctrectcorners
Introduced in R2019b
1-20
jpdaEvents
jpdaEvents
Feasible joint events for trackerJPDA
Syntax
FJE = jpdaEvents(validationMatrix)
[FJE,FJEProbs] = jpdaEvents(likelihoodMatrix,k)
Description
FJE = jpdaEvents(validationMatrix) returns the feasible joint events, FJE, based on the
validation matrix. A validation matrix describes the possible associations between detections and
tracks, whereas a feasible joint event for multi-object tracking is one realization of the associations
between detections and tracks.
Examples
Define an arbitrary validation matrix for five measurements and six tracks.
M = [1 1 1 1 1 0 1
1 0 1 1 0 0 0
1 0 0 0 1 1 0
1 1 1 1 0 0 0
1 1 1 1 1 1 1];
Generate all feasible joint events and count the total number.
FJE = jpdaEvents(M);
nFJE = size(FJE,3);
1-21
1 Functions
1 0 0 0 0 0 0
1 0 0 0 0 0 0
1 0 0 0 0 0 0
1 0 0 0 0 0 0
1 0 0 0 0 0 0
0 0 1 0 0 0 0
1 0 0 0 0 0 0
0 0 0 0 1 0 0
0 1 0 0 0 0 0
0 0 0 1 0 0 0
0 0 0 0 1 0 0
0 0 1 0 0 0 0
0 0 0 0 0 1 0
1 0 0 0 0 0 0
0 0 0 0 0 0 1
0 0 0 0 0 0 1
0 0 0 1 0 0 0
0 0 0 0 1 0 0
1 0 0 0 0 0 0
0 1 0 0 0 0 0
1 0 0 0 0 0 0
0 0 1 0 0 0 0
1 0 0 0 0 0 0
0 0 0 1 0 0 0
0 0 0 0 0 0 1
1 0 0 0 0 0 0
1 0 0 0 0 0 0
1 0 0 0 0 0 0
1 0 0 0 0 0 0
0 0 0 0 0 0 1
Generate three most probable events and obtain their normalized probabilities.
1-22
jpdaEvents
[FJE,FJEProbs] = jpdaEvents(likeMatrix,3)
1 0 0
1 0 0
0 1 0
0 0 1
FJE(:,:,2) =
0 0 1
1 0 0
0 1 0
1 0 0
FJE(:,:,3) =
1 0 0
0 1 0
1 0 0
0 0 1
FJEProbs = 3×1
0.4286
0.2857
0.2857
Input Arguments
validationMatrix — Validation matrix
m-by-(n+1) matrix
Validation matrix, specified as an m-by-(n+1) matrix, where m is the number of detections within the
cluster of a sensor scan, and n is the number of tracks maintained in the tracker. The validation
matrix uses the first column to account for the possibility that each detection is clutter or false alarm,
which is commonly referred to as "Track 0" or T0. The validation matrix is a binary matrix listing all
possible detections-to-track associations. If it is possible to assign track Ti to detection Dj, then the (j,
i+1) entry of the validation matrix is 1. Otherwise, the entry is 0.
Data Types: logical
Likelihood matrix, specified as an (m+1)-by-(n+1) matrix, where m is the number of detections within
the cluster of a sensor scan, and n is the number of tracks maintained in the tracker. The likelihood
matrix uses the first column to account for the possibility that each detection is clutter or false alarm,
which is commonly referred to as "Track 0" or T0. The matrix uses the first row to account for the
1-23
1 Functions
possibility that each track is not assigned to any detection, which can be referred to as "Detection 0"
or D0 . The (j+1,i+1) element of the matrix represents the likelihood to assign track Ti to detection Dj.
Data Types: logical
Output Arguments
FJE — Feasible joint events
m-by-(n+1)-by-p array
Feasible joint events, specified as an m-by-(n+1)-by-p array, where m is the number of detections
within the cluster of a sensor scan, n is the number of tracks maintained in the tracker, and p is the
total number of feasible joint events. Each page (an m-by-(n+1) matrix) of FJE corresponds to one
possible association between all the tracks and detections. The feasible joint event matrix on each
page satisfies:
For more details on feasible joint events, see “Feasible Joint Events” on page 1-24.
Data Types: logical
Probabilities of feasible joint events, returned as a p-by-1 vector of nonnegative scalars. The
summation of these scalars is equal to 1. The k-th element represents the probability of the k-th joint
events (specified in the FJE output argument) normalized over the p feasible joint events.
Data Types: logical
More About
Feasible Joint Events
In the typical workflow for a tracking system, the tracker needs to determine if a detection can be
associated with any of the existing tracks. If the tracker only maintains one track, the assignment can
be done by evaluating the validation gate around the predicted measurement and deciding if the
measurement falls within the validation gate. In the measurement space, the validation gate is a
spatial boundary, such as a 2-D ellipse or a 3-D ellipsoid, centered at the predicted measurement. The
validation gate is defined using the probability information (state estimation and covariance, for
example) of the existing track, such that the correct or ideal detections have high likelihood (97%
probability, for example) of falling within this validation gate.
However, if a tracker maintains multiple tracks, the data association process becomes more
complicated, because one detection can fall within the validation gates of multiple tracks. For
example, in the following figure, tracks T1 and T2 are actively maintained in the tracker, and each of
1-24
jpdaEvents
them has its own validation gate. Since the detection D2 is in the intersection of the validation gates
of both T1 and T2, the two tracks (T1 and T2) are connected and form a cluster. A cluster is a set of
connected tracks and their associated detections.
To represent the association relationship in a cluster, the validation matrix is commonly used. Each
row of the validation matrix corresponds to a detection while each column corresponds to a track. To
account for the eventuality of each detection being clutter, a first column is added and usually
referred to as "Track 0" or T0. If detection Di is inside the validation gate of track Dj, then the (j, i+1)
entry of the validation matrix is 1. Otherwise, it is zero. For the cluster shown in the figure, the
validation matrix Ω is
1 10
Ω= 1 1 1
1 01
Note that all the elements in the first column of Ω are 1, because any detection can be clutter or false
alarm. One important step in the logic of joint probabilistic data association (JPDA) is to obtain all the
feasible independent joint events in a cluster. Two assumptions for the feasible joint events are:
Based on these two assumptions, feasible joint events (FJEs) can be formulated. Each FJE is mapped
to an FJE matrix Ωp from the initial validation matrix Ω. For example, with the validation matrix Ω,
eight FJE matrices can be obtained:
1 0 0 010 1 00 1 0 0
Ω1 = 1 0 0 , Ω2 = 1 0 0 , Ω3 = 0 1 0 , Ω4 = 0 0 1
1 0 0 100 1 00 1 0 0
0 1 0 100 01 0 1 0 0
Ω5 = 0 0 1 , Ω6 = 1 0 0 , Ω7 = 1 0 0 , Ω8 = 0 1 0
1 0 0 001 00 1 0 0 1
As a direct consequence of the two assumptions, the Ωp matrices have exactly one "1" value per row.
Also, except for the first column which maps to clutter, there can be at most one "1" per column.
When the number of connected tracks grows in a cluster, the number of FJE increases rapidly. The
jpdaEvents function uses an efficient depth-first search algorithm to generate all the feasible joint
event matrices.
1-25
1 Functions
References
[1] Zhou, Bin, and N. K. Bose. "Multitarget tracking in clutter: Fast algorithms for data association."
IEEE Transactions on aerospace and electronic systems 29, no. 2 (1993): 352-363.
[2] Fisher, James L., and David P. Casasent. "Fast JPDA multitarget tracking algorithm." Applied optics
28, no. 2 (1989): 371-376.
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
• When dynamic memory allocation is disabled in the generated code, the order of events with the
same probability can be different from the results in MATLAB.
See Also
trackerJPDA
Introduced in R2019a
1-26
tunernoise
tunernoise
Noise structure of fusion filter
Syntax
noiseStruct = tunernoise(filterName)
Description
noiseStruct = tunernoise(filterName) returns the measurement noise structure for the filter
with name specified by the filterName input.
Examples
Create timetables for the sensor data and the truth data.
sensorData = timetable(Accelerometer, Gyroscope, ...
Magnetometer, GPSPosition, GPSVelocity, 'SampleRate', 100);
groundTruth = timetable(Orientation, Position, ...
'SampleRate', 100);
1-27
1 Functions
Create a tuner configuration object for the filter. Set the maximum iterations to two. Also, set the
tunable parameters as the unspecified properties.
config = tunerconfig('insfilterAsync','MaxIterations',8);
config.TunableParameters = setdiff(config.TunableParameters, ...
{'GeomagneticVectorNoise', 'AccelerometerBiasNoise', ...
'GyroscopeBiasNoise', 'MagnetometerBiasNoise'});
config.TunableParameters
Use the tuner noise function to obtain a set of initial sensor noises used in the filter.
measNoise = tunernoise('insfilterAsync')
1-28
tunernoise
3 QuaternionNoise 1.2943
3 VelocityNoise 1.2943
4 AccelerationNoise 1.2906
4 AccelerometerNoise 1.2836
4 AngularVelocityNoise 1.2491
4 GPSPositionNoise 1.2258
4 GPSVelocityNoise 1.1880
4 GyroscopeNoise 1.1701
4 MagnetometerNoise 1.1698
4 PositionNoise 1.1698
4 QuaternionNoise 1.1688
4 VelocityNoise 1.1688
5 AccelerationNoise 1.1650
5 AccelerometerNoise 1.1569
5 AngularVelocityNoise 1.1454
5 GPSPositionNoise 1.1100
5 GPSVelocityNoise 1.0778
5 GyroscopeNoise 1.0709
5 MagnetometerNoise 1.0675
5 PositionNoise 1.0675
5 QuaternionNoise 1.0669
5 VelocityNoise 1.0669
6 AccelerationNoise 1.0634
6 AccelerometerNoise 1.0549
6 AngularVelocityNoise 1.0549
6 GPSPositionNoise 1.0180
6 GPSVelocityNoise 0.9866
6 GyroscopeNoise 0.9810
6 MagnetometerNoise 0.9775
6 PositionNoise 0.9775
6 QuaternionNoise 0.9768
6 VelocityNoise 0.9768
7 AccelerationNoise 0.9735
7 AccelerometerNoise 0.9652
7 AngularVelocityNoise 0.9652
7 GPSPositionNoise 0.9283
7 GPSVelocityNoise 0.8997
7 GyroscopeNoise 0.8947
7 MagnetometerNoise 0.8920
7 PositionNoise 0.8920
7 QuaternionNoise 0.8912
7 VelocityNoise 0.8912
8 AccelerationNoise 0.8885
8 AccelerometerNoise 0.8811
8 AngularVelocityNoise 0.8807
8 GPSPositionNoise 0.8479
8 GPSVelocityNoise 0.8238
8 GyroscopeNoise 0.8165
8 MagnetometerNoise 0.8165
8 PositionNoise 0.8165
8 QuaternionNoise 0.8159
8 VelocityNoise 0.8159
dt = seconds(diff(groundTruth.Time));
N = size(sensorData,1);
qEst = quaternion.zeros(N,1);
1-29
1 Functions
posEst = zeros(N,3);
% Iterate the filter for prediction and correction using sensor data.
for ii=1:N
if ii ~= 1
predict(filter, dt(ii-1));
end
if all(~isnan(Accelerometer(ii,:)))
fuseaccel(filter,Accelerometer(ii,:), ...
tunedParams.AccelerometerNoise);
end
if all(~isnan(Gyroscope(ii,:)))
fusegyro(filter, Gyroscope(ii,:), ...
tunedParams.GyroscopeNoise);
end
if all(~isnan(Magnetometer(ii,1)))
fusemag(filter, Magnetometer(ii,:), ...
tunedParams.MagnetometerNoise);
end
if all(~isnan(GPSPosition(ii,1)))
fusegps(filter, GPSPosition(ii,:), ...
tunedParams.GPSPositionNoise, GPSVelocity(ii,:), ...
tunedParams.GPSVelocityNoise);
end
[posEst(ii,:), qEst(ii,:)] = pose(filter);
end
rmsorientationError = 2.7801
rmspositionError = 0.5966
figure();
t = (0:N-1)./ groundTruth.Properties.SampleRate;
subplot(2,1,1)
plot(t, positionError, 'b');
title("Tuned insfilterAsync" + newline + "Euclidean Distance Position Error")
xlabel('Time (s)');
ylabel('Position Error (meters)')
subplot(2,1,2)
plot(t, orientationError, 'b');
title("Orientation Error")
xlabel('Time (s)');
ylabel('Orientation Error (degrees)');
1-30
tunernoise
Input Arguments
filterName — Name of fusion filter
'insfilterAsync' | 'ahrs10filter' | 'insfilterMARG' | 'insfilterNonholonomic' |
'insfitlerErrorState'
• 'ahrs10filter'
• 'insfilterAsync'
• 'insfilterMARG'
• 'insfitlerErrorState'
• 'insfilterNonholonomic'
Output Arguments
noiseStruct — Structure of measurement noise
structure
Structure of measurement noise, returned as a structure. For the insfilterAsync object, the
structure contains these fields.
1-31
1 Functions
To use this structure with the tune function, change the values of the noise to proper values as initial
guesses for tuning the noise.
See Also
Introduced in R2020b
1-32
insfilter
insfilter
Create inertial navigation filter
Syntax
filter = insfilter
filter = insfilter('ReferenceFrame',RF)
Description
filter = insfilter returns an insfilterMARG inertial navigation filter object that estimates
pose based on accelerometer, gyroscope, GPS, and magnetometer measurements. See
insfilterMARG for more details.
Examples
The default INS filter is the insfilterMARG object. Call insfilter with no input arguments to
create the default INS filter.
filter = insfilter
filter =
insfilterMARG with properties:
IMUSampleRate: 100 Hz
ReferenceLocation: [0 0 0] [deg deg m]
State: [22x1 double]
StateCovariance: [22x22 double]
1-33
1 Functions
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
See Also
imufilter | ahrsfilter | insfilterErrorState | insfilterAsync |
insfilterNonholonomic | insfilterMARG
Topics
“Estimate Position and Orientation of a Ground Vehicle”
Introduced in R2018b
1-34
ecompass
ecompass
Orientation from magnetometer and accelerometer readings
Syntax
orientation = ecompass(accelerometerReading,magnetometerReading)
orientation = ecompass(accelerometerReading,magnetometerReading,
orientationFormat)
orientation = ecompass(accelerometerReading,magnetometerReading,
orientationFormat,'ReferenceFrame',RF)
Description
orientation = ecompass(accelerometerReading,magnetometerReading) returns a
quaternion that can rotate quantities from a parent (NED) frame to a child (sensor) frame.
orientation = ecompass(accelerometerReading,magnetometerReading,
orientationFormat) specifies the orientation format as quaternion or rotation matrix.
orientation = ecompass(accelerometerReading,magnetometerReading,
orientationFormat,'ReferenceFrame',RF) also allows you to specify the reference frame RF
of the orientation output. Specify RF as 'NED' (North-East-Down) or 'ENU' (East-North-Up). The
default value is 'NED'.
Examples
Use the known magnetic field strength and proper acceleration of a device pointed true north in
Boston to determine the magnetic declination of Boston.
Pass the magnetic field strength and acceleration to the ecompass function. The ecompass function
returns a quaternion rotation operator. Convert the quaternion to Euler angles in degrees.
q = ecompass(properAcceleration,magneticFieldStrength);
e = eulerd(q,'ZYX','frame');
The angle, e, represents the angle between true north and magnetic north in Boston. By convention,
magnetic declination is negative when magnetic north is west of true north. Negate the angle to
determine the magnetic declination.
magneticDeclinationOfBoston = -e(1)
magneticDeclinationOfBoston = -14.6563
1-35
1 Functions
The ecompass function fuses magnetometer and accelerometer data to return a quaternion that,
when used within a quaternion rotation operator, can rotate quantities from a parent (NED) frame to
a child frame. The ecompass function can also return rotation matrices that perform equivalent
rotations as the quaternion operator.
Define a rotation that can take a parent frame pointing to magnetic north to a child frame pointing to
geographic north. Define the rotation as both a quaternion and a rotation matrix. Then, convert the
quaternion and rotation matrix to Euler angles in degrees for comparison.
Define the magnetic field strength in microteslas in Boston, MA, when pointed true north.
Determine the quaternion and rotation matrix that is capable of rotating a frame from magnetic north
to true north. Display the results for comparison.
q = ecompass(a,m);
quaterionEulerAngles = eulerd(q,'ZYX','frame')
quaterionEulerAngles = 1×3
14.6563 0 0
r = ecompass(a,m,'rotmat');
theta = -asin(r(1,3));
psi = atan2(r(2,3)/cos(theta),r(3,3)/cos(theta));
rho = atan2(r(1,2)/cos(theta),r(1,1)/cos(theta));
rotmatEulerAngles = rad2deg([rho,theta,psi])
rotmatEulerAngles = 1×3
14.6563 0 0
Use ecompass to determine the gravity vector based on data from a rotating IMU.
Determine the orientation of the sensor body relative to the local NED frame over time.
orientation = ecompass(sensorData.Acceleration,sensorData.MagneticField);
To estimate the gravity vector, first rotate the accelerometer readings from the sensor body frame to
the NED frame using the orientation quaternion vector.
1-36
ecompass
gravityVectors = rotatepoint(orientation,sensorData.Acceleration);
Determine the gravity vector as an average of the recovered gravity vectors over time.
gravityVectorEstimate = mean(gravityVectors,1)
gravityVectorEstimate = 1×3
Fuse modeled accelerometer and gyroscope data to track a spinning platform using both idealized
and realistic data.
Describe the ground-truth orientation of the platform over time. Use the kinematicTrajectory on
page 3-201 System object™ to create a trajectory for a platform that has no translation and spins
about its z-axis.
duration = 12;
fs = 100;
numSamples = fs*duration;
accelerationBody = zeros(numSamples,3);
angularVelocityBody = zeros(numSamples,3);
zAxisAngularVelocity = [linspace(0,4*pi,4*fs),4*pi*ones(1,4*fs),linspace(4*pi,0,4*fs)]';
angularVelocityBody(:,3) = zAxisAngularVelocity;
trajectory = kinematicTrajectory('SampleRate',fs);
[~,orientationNED,~,accelerationNED,angularVelocityNED] = trajectory(accelerationBody,angularVelo
Use an imuSensor on page 3-587 System object to mimic data received from an IMU that contains
an ideal magnetometer and an ideal accelerometer.
IMU = imuSensor('accel-mag','SampleRate',fs);
[accelerometerData,magnetometerData] = IMU(accelerationNED, ...
angularVelocityNED, ...
orientationNED);
Pass the accelerometer data and magnetometer data to the ecompass function to estimate
orientation over time. Convert the orientation to Euler angles in degrees and plot the result.
orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');
timeVector = (0:numSamples-1).'/fs;
1-37
1 Functions
figure(1)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Ideal IMU')
Modify parameters of the IMU System object to approximate realistic IMU sensor data. Reset the IMU
and then call it with the same ground-truth acceleration, angular velocity, and orientation. Use
ecompass to fuse the IMU data and plot the results.
IMU.Accelerometer = accelparams( ...
'MeasurementRange',20, ...
'Resolution',0.0006, ...
'ConstantBias',0.5, ...
'AxesMisalignment',2, ...
'NoiseDensity',0.004, ...
'BiasInstability',0.5);
IMU.Magnetometer = magparams( ...
'MeasurementRange',200, ...
'Resolution',0.01);
reset(IMU)
[accelerometerData,magnetometerData] = IMU(accelerationNED,angularVelocityNED,orientationNED);
1-38
ecompass
orientation = ecompass(accelerometerData,magnetometerData);
orientationEuler = eulerd(orientation,'ZYX','frame');
figure(2)
plot(timeVector,orientationEuler)
legend('z-axis','y-axis','x-axis')
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation from Realistic IMU')
Input Arguments
accelerometerReading — Accelerometer readings in sensor body coordinate system (m/s2)
N-by-3 matrix
Accelerometer readings in sensor body coordinate system in m/s2, specified as an N-by-3 matrix. The
columns of the matrix correspond to the x-, y-, and z-axes of the sensor body. The rows in the matrix,
N, correspond to individual samples. The accelerometer readings are normalized before use in the
function.
Data Types: single | double
Magnetometer readings in sensor body coordinate system in µT, specified as an N-by-3 matrix. The
columns of the matrix correspond to the x-, y-, and z-axes of the sensor body. The rows in the matrix,
1-39
1 Functions
N, correspond to individual samples. The magnetometer readings are normalized before use in the
function.
Data Types: single | double
Output Arguments
orientation — Orientation that rotates quantities from global coordinate system to sensor
body coordinate system
N-by-1 vector of quaternions (default) | 3-by-3-by-N array
Orientation that can rotate quantities from a global coordinate system to a body coordinate system,
returned as a vector of quaternions or an array. The size and type of the orientation depends on
the format used to describe orientation:
• 'quaternion' –– N-by-1 vector of quaternions with the same underlying data type as the input
• 'rotmat' –– 3-by-3-by-N array the same data type as the input
Algorithms
The ecompass function returns a quaternion or rotation matrix that can rotate quantities from a
parent (NED for example) frame to a child (sensor) frame. For both output orientation formats, the
rotation operator is determined by computing the rotation matrix.
R = (a × m) × a a × m a
and then normalized column-wise. a and m are the accelerometerReading input and the
magnetometerReading input, respectively.
To understand the rotation matrix calculation, consider an arbitrary point on the Earth and its
corresponding local NED frame. Assume a sensor body frame, [x,y,z], with the same origin.
1-40
ecompass
Recall that orientation of a sensor body is defined as the rotation operator (rotation matrix or
quaternion) required to rotate a quantity from a parent (NED) frame to a child (sensor body) frame:
R pparent = pchild
where
• R is a 3-by-3 rotation matrix, which can be interpreted as the orientation of the child frame.
• pparent is a 3-by-1 vector in the parent frame.
• pchild is a 3-by-1 vector in the child frame.
For a stable sensor body, an accelerometer returns the acceleration due to gravity. If the sensor body
is perfectly aligned with the NED coordinate system, all acceleration due to gravity is along the z-
axis, and the accelerometer reads [0 0 1]. Consider the rotation matrix required to rotate a quantity
from the NED coordinate system to a quantity indicated by the accelerometer.
1-41
1 Functions
The third column of the rotation matrix corresponds to the accelerometer reading:
r31 a1
r32 = a2
r33 a3
A magnetometer reading points toward magnetic north and is in the N-D plane. Again, consider a
sensor body frame aligned with the NED coordinate system.
By definition, the E-axis is perpendicular to the N-D plane, therefore N ⨯ D = E, within some
amplitude scaling. If the sensor body frame is aligned with NED, both the acceleration vector from
the accelerometer and the magnetic field vector from the magnetometer lie in the N-D plane.
Therefore m ⨯ a = y, again with some amplitude scaling.
Consider the rotation matrix required to rotate NED to the child frame, [x y z].
The second column of the rotation matrix corresponds to the cross product of the accelerometer
reading and the magnetometer reading:
1-42
ecompass
r21 a1 m1
r22 = a2 × m2
r23 a3 m3
Ri j
Ri j = , ∀j
3
∑ Ri2j
i=1
Note The ecompass algorithm uses magnetic north, not true north, for the NED coordinate system.
References
[1] Open Source Sensor Fusion. https://github.com/memsindustrygroup/Open-Source-Sensor-Fusion/
tree/master/docs
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
See Also
quaternion | ahrsfilter | imufilter
Topics
“Determine Orientation Using Inertial Sensors”
Introduced in R2018b
1-43
1 Functions
magcal
Magnetometer calibration coefficients
Syntax
[A,b,expmfs] = magcal(D)
[A,b,expmfs] = magcal(D,fitkind)
Description
[A,b,expmfs] = magcal(D) returns the coefficients needed to correct uncalibrated
magnetometer data D.
To produce the calibrated magnetometer data C, use equation C = (D-b)*A. The calibrated data C
lies on a sphere of radius expmfs.
Examples
[x,y,z] = ellipsoid(c(1),c(2),c(3),r(1),r(2),r(3),20);
D = [x(:),y(:),z(:)];
Correct the magnetometer data so that it lies on a sphere. The option for the calibration is set by
default to 'auto'.
expmfs = 31.0723
figure(1)
plot3(x(:),y(:),z(:),'LineStyle','none','Marker','X','MarkerSize',8)
hold on
grid(gca,'on')
plot3(C(:,1),C(:,2),C(:,3),'LineStyle','none','Marker', ...
'o','MarkerSize',8,'MarkerFaceColor','r')
axis equal
xlabel('uT')
1-44
magcal
ylabel('uT')
zlabel('uT')
legend('Uncalibrated Samples', 'Calibrated Samples','Location', 'southoutside')
title("Uncalibrated vs Calibrated" + newline + "Magnetometer Measurements")
hold off
Input Arguments
D — Raw magnetometer data
N-by-3 matrix (default)
Input matrix of raw magnetometer data, specified as a N-by-3 matrix. Each column of the matrix
corresponds to the magnetometer measurements in the first, second and third axes, respectively.
Each row of the matrix corresponds to a single three-axis measurement.
Data Types: single | double
Matrix type for output A. The matrix type of A can be constrained to:
1-45
1 Functions
• 'sym' – symmetric
• 'auto' – whichever of the previous options gives the best fit
Output Arguments
A — Correction matrix for soft-iron effect
3-by-3 matrix
More About
Soft- and Hard-Iron Effects
Because a magnetometer usually rotates through a full range of 3-D rotation, the ideal measurements
from a magnetometer should form a perfect sphere centered at the origin if the magnetic field is
unperturbed. However, due to distorting magnetic fields from the sensor circuit board and the
surrounding environment, the spherical magnetic measurements can be perturbed. In general, two
effects exist.
1 The soft-iron effect is described as the distortion of the ellipsoid from a sphere and the tilt of the
ellipsoid, as shown in the left figure. This effect is caused by disturbances that influence the
magnetic field but may not generate their own magnetic field. For example, metals such as nickel
and iron can cause this kind of distortion.
2 The hard-iron effect is described as the offset of the ellipsoid center from the origin. This effect is
produced by materials that exhibit a constant, additive field to the earth's magnetic field. This
constant additive offset is in addition to the soft-iron effect as shown in the figure on the right.
The underlying algorithm in magcal determines the best-fit ellipsoid to the raw sensor readings and
attempts to "invert" the ellipsoid to produce a sphere. The goal is to generate a correction matrix A to
1-46
Another Random Scribd Document
with Unrelated Content
"Speak! speak!"
"This tree, at the foot of which we are now standing, although
apparently possessing all the vigour of youth, is internally hollow, so
that only the bark stands upright. Your wife and daughter, supplied
with some provisions, will get into the tree and remain there in
safety till the danger has passed away. As for ourselves—"
"As for us," Black quickly interrupted her, "we are men accustomed
to danger: our fate is in the hands of God."
"Good; but do not despair: all is not lost yet."
The American shook his head.
"As you said yourself, what can four men do against a legion of
demons like those who menace us? But that is not the question of
the moment. I do not see the hole by which my wife and daughter
can enter the tree."
"It is twenty to twenty-four feet up, hidden among the branches and
leaves."
"The Lord be praised! they will be sheltered."
"Yes; but make haste and warn them, while your son and I make all
the preparations."
Black, convinced of the necessity of haste, ran off, while the stranger
and William constructed, with that dexterity produced by the
approach of danger, a species of handy ladder, by which the two
women could not merely ascend the tree, but go down into the
cavity. Black waked the ladies, and called the servants; in a few
words he explained to them what was passing; then, loading his wife
and daughter with provisions, furs, and other indispensable objects,
he led them to the spot where the stranger was expecting them.
"This is my most precious treasure," Black said; "if I save it, I shall
be solely indebted to you."
The two ladies began thanking their mysterious protectress; but she
imposed silence on them by a peremptory gesture.
"Presently, presently," she said; "if we escape, we shall have plenty
of time for mutual congratulations; but at this moment we have
something more important to do than exchange compliments. We
must get into a place of safety."
The two ladies fell back, quite repulsed by this rough reception,
while casting a curious and almost alarmed glance on the strange
creature. But the latter, perfectly stoical, seemed to notice nothing.
She explained in a few clear words the means she had found to
conceal them: recommended them to remain silent in the hollow
tree, and then ordered them to mount. The two ladies, after
embracing Black and his son, began resolutely ascending the rungs
of the improvised ladder. They reached in a few seconds an
enormous branch, on which they stopped, by the orders of the
stranger. Black then threw down into the interior of the tree the furs
and provisions, after which the ladder was placed inside, and the
ladies glided through the hole.
"We leave you the ladder, which is useless to us," the stranger then
said. "But be very careful not to come out till you have seen me
again; the least imprudence, under the circumstances, might cost
your lives. However, keep your minds at rest. Your imprisonment will
not be long, a few hours at the most: so be of good cheer."
The ladies once again tried to express their gratitude; but, without
listening, the stranger made Black a sign to follow her, and rapidly
descended from the tree. Aided by the Americans, she then began
removing every trace that might have revealed where the ladies
were bestowed. When the stranger had assured herself, by a final
glance, that all was in order, and nothing could betray those who
were so famously hidden, she sighed, and followed by the two men,
walked to the intrenchments.
"Now," she said, "let us watch attentively around us, for these
demons will probably crawl close up in the shadows. You are free
and honest Americans, show these accursed Indians what you can
do."
"Let them come!" Black muttered hoarsely.
"They will soon do so," she replied, and pointed to several almost
imperceptible black dots, which, however, grew larger, and were
evidently approaching the encampment.
CHAPTER VI.
CHAPTER VII.
CHAPTER VIII.
THE EXILE.
We are compelled, for the proper comprehension of the facts that
will follow, to break off our story for a moment, in order to describe
a strange adventure which happened on the Western Prairies some
thirty odd years before our story opens.
The Indians, whom people insist so wrongly, in our opinion, in
regarding as savages, have certain customs which display a
thorough knowledge of the human heart. The Comanches, who
appear to remember that in old times they enjoyed a far advanced
civilization, have retained the largest amount of those customs which
are, certainly, stamped with originality.
One day in the month of February, which they call the Moon of the
Arriving Eagles, and in the year 1795 or 1796, a village of the Red
Cow tribe was in a state of extraordinary agitation. The hachesto, or
public speaker, mounted on the roof of a lodge, summoned the
warriors for the seventh hour of the day to the village square, near
the ark of the first man, where a grand council would be held. The
warriors asked each other in vain the purport of this unforeseen
meeting, but no one could tell them: the hachesto himself was
ignorant, and they were obliged to await the hour of assembling,
although the comments and suppositions still went on to a great
extent.
The Redskins, whom badly-informed authors represent to us as cold,
silent men, are, on the contrary, very gay, and remarkable gossips
when together. What has caused the contrary supposition is, that in
their relations with white men the Indians are, in the first place,
checked by the difficulties of the language—equally insurmountable,
by the way, for both parties—and next by the distrust which every
American native feels towards Europeans, whoever they may be,
owing to the inveterate hatred that separates the two races.
During our lengthened residence among Indian tribes we often had
opportunities for noticing what mistakes are made with respect to
the Redskins. During their long evening gossips in the villages, or
the hunting expeditions, there was a rolling fire of jokes and
witticisms, often lasting whole hours, to the great delight of the
audience, who laughed that hearty Indian laugh, without care or
afterthought, which cleaves the mouth to the ears, and draws tears
of delight,—a laugh which, for metallic resonance, can only be
compared with that of negroes, though the former is far more
spiritual than the latter, whose notes have ever something bestial
about them.
Toward the decline of day, the hour selected for the meeting, the
village square presented a most animated appearance. The warriors,
women, children, and dogs, those inseparable guests of the
Redskins, pressed round a large circle left empty in the centre for
the council fire, near which the principal chiefs of the nation
crouched ceremoniously. At a sign from an old sachem whose hair,
white as silver, fell in a cloud on his shoulders, the pipe bearer
brought in the great calumet, the stem of which he presented to
each chief in turn, while holding the bowl in the palm of his hand.
When all the chiefs had smoked, the pipe bearer turned the calumet
to the four cardinal points, while murmuring mysterious words which
no one heard; then he emptied the ash into the fire, saying aloud,—
"Chiefs, warriors, women, and children of the Red Cow, your
sachems are assembled to judge a very grave question; pray to the
Master of Life to inspire them with wise words."
Then the pipe bearer, after bowing respectfully to the chiefs,
withdrew, taking the calumet with him. The council began, and, at a
sign from the aged sachem, a chief rose, and bowing, took the
word:—
"Venerated sachems, chiefs, and warriors of my nation," he said, in a
loud voice, "the mission with which I am entrusted is painful to my
heart: listen to me indulgently, be not governed by passion; but let
justice alone preside over the severe decree which you will, perhaps,
be compelled to pronounce. The mission which I am entrusted with
is painful, I repeat; it fills my heart with sadness: I am compelled to
accuse before you two renowned chiefs belonging to two illustrious
families, who have, with equal claims, deserved well of the nation on
many occasions by rendering it signal services; these chiefs, as I
must name them before you, are the Bounding Panther, and the
Sparrow Hawk."
Welcome to our website – the perfect destination for book lovers and
knowledge seekers. We believe that every book holds a new world,
offering opportunities for learning, discovery, and personal growth.
That’s why we are dedicated to bringing you a diverse collection of
books, ranging from classic literature and specialized publications to
self-development guides and children's books.
ebookbell.com