Модель движения с постоянной скоростью (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 принимает один наблюдатель, и состояние задано относительно него.