Модель движения постоянной скорости (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
Задайте вектор состояния для модели движения в 2D. Временной интервал составляет 2 секунды.
mscState = [0.5;0.02;1/1000;-10/1000]; dt = 2;
Когда состояние модифицированных сферических координат (MSC) относительно, позвольте наблюдателю утвердить быть заданными постоянной ускоряющей моделью в 2D.
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 принимает одного наблюдателя, и состояние задано относительно нее.