Модель движения с постоянной скоростью (CV) в кадре MSC
Определите вектор состояния для 3-D состояния MSC.
mscState = [0.1;0.01;0.1;0.01;0.001;1]; dt = 0.1;
Предсказать состояние с нулевым ускорением наблюдателя.
mscState = constvelmsc(mscState,zeros(3,1),dt)
mscState = 6×1
0.1009
0.0083
0.1009
0.0083
0.0009
0.9091
Спрогнозировать состояние с помощью [5; 3; 1] ускорения наблюдателя в сценарии.
mscState = constvelmsc(mscState,zeros(3,1),dt,[5;3;1])
mscState = 6×1
0.1017
0.0067
0.1017
0.0069
0.0008
0.8329
Спрогнозировать состояние с маневром наблюдателя и единичным среднеквадратическим случайным шумом в целевом ускорении. Пусть ускорение наблюдателя в интервале времени будет )].
velManeuver = [1 - cos(dt);sin(dt);0]; posManeuver = [-sin(dt);cos(dt) - 1;0]; u = zeros(6,1); u(1:2:end) = posManeuver; u(2:2:end) = velManeuver; mscState = constvelmsc(mscState,randn(3,1),dt,u)
mscState = 6×1
0.1023
0.0058
0.1023
0.0057
0.0008
0.7689
Определите вектор состояния для модели движения в 2-D. Интервал времени составляет 2 секунды.
mscState = [0.5;0.02;1/1000;-10/1000]; dt = 2;
Поскольку измененное состояние сферических координат (MSC) является относительным, пусть состояние наблюдателя определяется моделью постоянного ускорения в 2-D.
observerState = [100;10;0.5;20;-5;0.1];
Предварительное выделение памяти. rPlot - диапазон для печати измерений подшипника.
observerPositions = zeros(2,10); targetPositions = zeros(2,10); azimuthMeasurement = zeros(1,10); bearingHistory = zeros(2,30); rPlot = 2000;
Используйте цикл для прогнозирования состояния несколько раз. Использовать constvelmsc для создания траектории с целью постоянной скорости и измерения углов с помощью функции измерения, cvmeasmsc.
for i = 1:10 obsAcceleration = observerState(3:3:end); % Use zeros(2,1) as process noise to get true predictions mscState = constvelmsc(mscState,zeros(2,1),dt,obsAcceleration); % Update observer state using constant acceleration model observerState = constacc(observerState,dt); observerPositions(:,i) = observerState(1:3:end); % Update bearing history with current measurement. az = cvmeasmsc(mscState); bearingHistory(:,3*i-2) = observerState(1:3:end); bearingHistory(:,3*i-1) = observerState(1:3:end) + [rPlot*cosd(az);rPlot*sind(az)]; bearingHistory(:,3*i) = [NaN;NaN]; % Use the 'rectangular' frame to get relative positions of the % target using cvmeasmsc function. relativePosition = cvmeasmsc(mscState,'rectangular'); relativePosition2D = relativePosition(1:2); targetPositions(:,i) = relativePosition2D + observerPositions(:,i); end
plot(observerPositions(1,:),observerPositions(2,:)); hold on; plot(targetPositions(1,:),targetPositions(2,:)); plot(bearingHistory(1,:),bearingHistory(2,:),'-.'); title('Constant velocity model in modified spherical coordinates');xlabel('X[m]'); ylabel('Y[m]') legend('Observer Positions', 'Target Positions', 'Bearings Measurements'); hold off;
Функция обеспечивает функцию перехода с постоянной скоростью в измененных сферических координатах (MSC) с использованием неаддитивной шумовой структуры. Кадр MSC предполагает наличие одного наблюдателя, и состояние определяется относительно него.