-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathintroduceNewPTs.m
67 lines (50 loc) · 3.39 KB
/
introduceNewPTs.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
% Florian Meyer, 2017, 2020
function [newPTs,newLabels,newExistences,xiMessages] = introduceNewPTs(newMeasurements,sensor,step,unknownNumber,unknownParticles,parameters)
numParticles = parameters.numParticles;
detectionProbability = parameters.detectionProbability;
clutterIntensity = parameters.meanClutter*parameters.clutterDistribution;
sensorPositions = parameters.sensorPositions(:,sensor);
numMeasurements = size(newMeasurements,2);
% compute unknown intensity and perform update step for unknown targets
unknownIntensity = unknownNumber/((parameters.surveillanceRegion(1,1)-parameters.surveillanceRegion(2,1))*(parameters.surveillanceRegion(1,2)-parameters.surveillanceRegion(2,2)));
unknownIntensity = unknownIntensity*(1-detectionProbability)^(sensor-1);
if(numMeasurements)
constants = calculateConstantsUniform(sensorPositions, newMeasurements, unknownParticles, parameters);
end
% introduce new PTs and compute xi messages
newPTs = zeros(4,numParticles,numMeasurements);
newLabels = zeros(3,numMeasurements);
xiMessages = zeros(numMeasurements,1);
for measurement = 1:numMeasurements
newPTs(:,:,measurement) = sampleFromLikelihood(newMeasurements(:,measurement), sensor, numParticles, parameters);
newLabels(:,measurement) = [step;sensor;measurement];
xiMessages(measurement) = 1 + (constants(measurement) * unknownIntensity * detectionProbability)/clutterIntensity;
end
newExistences = xiMessages - 1;
end
function [constants] = calculateConstantsUniform(sensorPosition, newMeasurements, particles, parameters)
measurementVarianceRange = parameters.measurementVarianceRange;
measurementVarianceBearing = parameters.measurementVarianceBearing;
numMeasurements = size(newMeasurements,2);
numParticles = size(particles,2);
constantWeight = 1/((parameters.surveillanceRegion(1,1)-parameters.surveillanceRegion(2,1))*(parameters.surveillanceRegion(1,2)-parameters.surveillanceRegion(2,2)));
predictedRange = sqrt((particles(1,:) - sensorPosition(1)).^2+(particles(2,:) - sensorPosition(2)).^2);
predictedBearing = atan2d(particles(1,:) - sensorPosition(1), particles(2,:) - sensorPosition(2));
constantLikelihood = 1/(2*pi*sqrt(measurementVarianceBearing*measurementVarianceRange));
constants = zeros(numMeasurements,1);
for measurement = 1:numMeasurements
constants(measurement) = sum(1/numParticles*constantLikelihood*exp((-1/2)*(repmat(newMeasurements(1,measurement),1,numParticles) - predictedRange).^2/(measurementVarianceRange)).*exp((-1/2)*(repmat(newMeasurements(2,measurement),1,numParticles) - predictedBearing).^2/(measurementVarianceBearing)));
end
constants = constants/constantWeight;
end
function [ samples ] = sampleFromLikelihood(measurement, sensorIndex, numParticles, parameters)
sensorPosition = parameters.sensorPositions(:,sensorIndex);
measurementVarianceRange = parameters.measurementVarianceRange;
measurementVarianceBearing = parameters.measurementVarianceBearing;
priorVelocityCovariance = parameters.priorVelocityCovariance;
samples = zeros(4,numParticles);
randomRange = measurement(1)+sqrt(measurementVarianceRange)*randn(1,numParticles);
randomBearing = measurement(2)+sqrt(measurementVarianceBearing)*randn(1,numParticles);
samples(1:2,:) = [sensorPosition(1) + randomRange.*sind(randomBearing); sensorPosition(2) + randomRange.*cosd(randomBearing)];
samples(3:4,:) = sqrtm(priorVelocityCovariance) * randn(2,numParticles);
end