-
Notifications
You must be signed in to change notification settings - Fork 2
/
GenerateTargetMotion.m
69 lines (50 loc) · 1.83 KB
/
GenerateTargetMotion.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
function [ TrueState, TargSpecOut ] = GenerateTargetMotion(TargSpec)
%GENERATETARGETMOTION Use a target specification to generate multi-target
%state sets for each frame
global Par;
% Input: TargSpec: structure array specifying target existence and dynamics
% .birth - birth time
% .death - death time
% .state - initial birth state
% .acc - array of deterministic accelerations
% Output: TrueState: cell array of track objects
% Initialise State cell array
TrueState = cell(Par.NumTgts, 1);
% Generate tracks
for j = 1:Par.NumTgts
birth = TargSpec(j).birth;
death = TargSpec(j).death;
num = death - birth;
% Create track arrays
state = cell(num, 1);
assoc = zeros(num, 1);
% First frame
state{1} = TargSpec(j).state;
% Loop through frames
for k = 2:num
% [bng, range] = Cart2Pol(state{k-1});
% if range > 0.75*Par.Xmax
% magn = -0.01*(range - 0.75*Par.Xmax);
% acc = Pol2Cart(bng, magn);
% TargSpec(j).acc(k, :) = acc;
% end
% Calculate expected state
exp_state = Par.A * state{k-1} + Par.B * TargSpec(j).acc(k, :)';
% Sample state from Gaussian
state{k} = mvnrnd(exp_state', Par.Q)';
% Kill if outside scene
if any(state{k}(1:2)>Par.Xmax)||any(state{k}(1:2)<-Par.Xmax)
state(k:end) = [];
assoc(k:end) = [];
death = birth + k - 1;
num = death - birth;
TargSpec(j).death = death;
break;
end
% Limit velocity
state{k}(3:4) = min( max( state{k}(3:4), -Par.Vmax), Par.Vmax);
end
TrueState{j} = Track(birth, death, state, assoc);
end
TargSpecOut = TargSpec;
end