%% Tracking Maneuvering Targets
% This example shows how to track maneuvering targets using various
% tracking filters. The example shows the difference between filters that
% use a single motion model and multiple motion models.
% Copyright 2018 The MathWorks, Inc.
%% Define a Scenario
% In this example, you define a single target that initially travels at a
% constant velocity of 200 m/s for 33 seconds, then enters a constant turn
% of 10 deg/s. The turn lasts for 33 seconds, then the target accelerates
% in a straight line at 3 m/s^2.
[trueState, time, fig1] = helperGenerateTruthData;
dt = diff(time(1:2));
numSteps = numel(time);
figure(fig1)
%%
% Define the measurements to be the position and add normal random noise
% with a standard deviation of 1 to the measurements.
% Set the RNG seed for repeatable results
s = rng;
rng(2018);
positionSelector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; % Position from state
truePos = positionSelector * trueState;
measNoise = randn(size(truePos));
measPos = truePos + measNoise;
%% Track Using a Constant-Velocity Filter
% You define a |trackingEKF| with a constant-velocity motion model. You use
% the first measurement to define the initial state and state covariance,
% and set the process noise to be non-additive, to define the process noise
% in terms of the unknown acceleration in the x, y, and z components. This
% definition is similar to how the function |initcvekf| works.
initialState = positionSelector' * measPos(:,1);
initialCovariance = diag([1,1e4,1,1e4,1,1e4]); % Velocity is not measured
cvekf = trackingEKF(@constvel, @cvmeas, initialState, ...
'StateTransitionJacobianFcn', @constveljac, ...
'MeasurementJacobianFcn', @cvmeasjac, ...
'StateCovariance', initialCovariance, ...
'HasAdditiveProcessNoise', false, ...
'ProcessNoise', eye(3));
%%
% For each measurement, you predict the filter, calculate the distance of
% the predicted state from the true position, and correct the filter using
% the measurement to obtain the filtered estimate of the position.
dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
for i = 2:size(measPos,2)
predict(cvekf, dt);
dist(i) = distance(cvekf,truePos(:,i)); % Distance from true position
estPos(:,i) = positionSelector * correct(cvekf, measPos(:,i));
end
figure(fig1);
plot(estPos(1,:),estPos(2,:),'.g','DisplayName','CV Low PN')
title('True and Estimated Positions')
axis([5000 8000 -500 2500])
%%
% As depicted in the figure, the filter is able to track the constant
% velocity part of the motion very well, but when the target executes the
% turn, the filter estimated position diverges from the true position. You
% can see the distance of the estimate from the truth in the following plot.
% During the turn, at 33-66 seconds, the normalized distance jumps to very
% high values, which means that the filter is unable to track the
% maneuvering target.
fig2 = figure;
hold on
plot((1:numSteps)*dt,dist,'g','DisplayName', 'CV Low PN')
title('Normalized Distance From Estimated Position to True Position')
xlabel('Time (s)')
ylabel('Normalized Distance')
legend
%% Increase the Process Noise
% One possible solution is to increase the process noise. The process noise
% represents the unmodeled terms in the motion model. For a constant
% velocity model, these are the unknown acceleration terms. By increasing
% the process noise, you allow for larger uncertainty in the motion model,
% which causes the filter to rely on the measurements more than on the
% model. The following lines create a constant velocity filter, with high
% process noise values that correspond to about 5-G turn.
cvekf2 = trackingEKF(@constvel, @cvmeas, initialState, ...
'StateTransitionJacobianFcn', @constveljac, ...
'MeasurementJacobianFcn', @cvmeasjac, ...
'StateCovariance', initialCovariance, ...
'HasAdditiveProcessNoise', false, ...
'ProcessNoise', diag([50,50,1])); % Large uncertainty in the horizontal acceleration
dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
for i = 2:size(measPos,2)
predict(cvekf2, dt);
dist(i) = distance(cvekf2,truePos(:,i)); % Distance from true position
estPos(:,i) = positionSelector * correct(cvekf2, measPos(:,i));
end
figure(fig1)
plot(estPos(1,:),estPos(2,:),'.c','DisplayName','CV High PN')
axis([5000 8000 -500 2500])
%%
% Increasing the process noise significantly improves the filter's ability
% to track the target during the turn. However, there is a cost: the filter
% is less capable of smoothing out the measurement noise during the
% constant velocity period of the motion. Even though the normalized
% distance during the turn was significantly reduced, the normalized
% distance increased in the first 33 seconds, during the constant velocity
% period of the motion.
figure(fig2)
plot((1:numSteps)*dt,dist,'c','DisplayName', 'CV High PN')
axis([0 100 0 50])
%% Use an Interacting Motion-Model Filter
% Another solution is to use a filter that can consider all motion models
% at the same time, called an interacting multiple-model (IMM) filter. The
% IMM filter can maintain as many motion models as you want, but typically
% is used with 2-5 motion models. For this example, three models are
% sufficient: a constant velocity model, a constant turn model, and a
% constant acceleration model.
imm = trackingIMM('TransitionProbabilities', 0.99); % The default IMM has all three models
% Initialize the state and state covariance in terms of the first model
initialize(imm, initialState, initialCovariance);
%%
% You use the IMM filter in the same way that the EKF was used.
dist = zeros(1,numSteps);
estPos = zeros(3,numSteps);
modelProbs = zeros(3,numSteps);
modelProbs(:,1) = imm.ModelProbabilities;
for i = 2:size(measPos,2)
predict(imm, dt);
dist(i) = distance(imm,truePos(:,i)); % Distance from true position
estPos(:,i) = positionSelector * correct(imm, measPos(:,i));
modelProbs(:,i) = imm.ModelProbabilities;
end
figure(fig1)
plot(estPos(1,:),estPos(2,:),'.m','DisplayName','IMM')
%%
% The |trackingIMM| filter is able to track the maneuvering target in all
% three parts of the motion.
%
% Examining the distance between the filter's predicted state and the true
% position, you see that the IMM filter is able to reduce the distance for
% all portions of the motion. In fact, the IMM filter is better at tracking
% the motion than both other constant velocity models used earlier.
figure(fig2)
hold on
plot((1:numSteps)*dt,dist,'m','DisplayName', 'IMM')
axis([0 100 0 50])
%%
% To better understand how the IMM filter works, plot the model
% probabilities as a function of time. The figure shows that filter is
% initialized with the three models having the same probability. As the
% filter is updated, it quickly converges to a very high probability that
% the model is a constant velocity model. After 33 seconds of motion, the
% constant velocity model is no longer true, and the probability of the
% constant turn model becomes very high for the duration of the turn. In
% the last section of the motion, during the constant acceleration
% maneuver, the IMM filter assigns a high probability that the motion is
% constant acceleration, but the filter is less certain about the correct
% motion model, and there is about 0.3 probability of a constant velocity
% motion.
figure
plot((1:numSteps)*dt, modelProbs)
title('Model Probabilities vs. Time')
xlabel('Time (s)')
ylabel('Model Probabilities')
legend('IMM-CV','IMM-CA','IMM-CT')
% Return the RNG to its previous state
rng(s)
%% Summary
% This example showed you how to track a target maneuvering with constant
% turn and constant acceleration motion. The example showed how you can
% increase the process noise to capture the unknown maneuver with a
% constant velocity model. You also saw how to improve the tracking of a
% maneuvering target by using an IMM filter.
%% Supporting Functions
%%%
% *|helperGenerateTruthData|*
%
% This function generates the ground truth tr
没有合适的资源?快使用搜索试试~ 我知道了~
温馨提示
此示例演示如何使用各种跟踪筛选器跟踪机动目标。该示例显示了使用单个运动模型和多个运动模型的滤镜之间的差异。 在此示例中,定义了一个目标,该目标最初以 200 m/s 的恒定速度行进 33 秒,然后输入 10 度/秒的恒定转弯。转弯持续33秒,然后目标以3 m / s ^ 2的速度直线加速。 定义具有等速运动模型的 。使用第一个测量值来定义初始状态和状态协方差,并将过程噪声设置为非累加,以根据 x、y 和 z 分量中的未知加速度来定义过程噪声。此定义类似于函数的工作方式。 对于每个测量值,可以预测滤波器,计算预测状态与真实位置的距离,并使用测量值校正滤波器以获得位置的滤波估计值。 一种可能的解决方案是增加过程噪音。过程噪声表示运动模型中的未建模项。对于等速模型,这些是未知的加速度项。通过增加过程噪声,可以增加运动模型中的不确定性,从而导致滤波器更多地依赖于测量结果而不是模型。以下线条创建一个等速滤波器,其高过程噪声值对应于约 5-G 转数。
资源推荐
资源详情
资源评论
收起资源包目录
跟踪机动目标仿真.rar (2个子文件)
跟踪机动目标仿真
TrackingManeuveringTargetsExample.m 9KB
helperGenerateTruthData.m 1KB
共 2 条
- 1
资源评论
- zhangrongzhen1232023-12-21资源内容总结的很到位,内容详实,很受用,学到了~
- d_b2232023-04-14实在是宝藏资源、宝藏分享者!感谢大佬~
珞瑜·
- 粉丝: 11w+
- 资源: 500
下载权益
C知道特权
VIP文章
课程特权
开通VIP
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功