Пример 1: Преобразование обнаружений в стационарной прямоугольной системе координат

Рассмотрим 2-D сценарий слежения со стационарной башней и грузовиком. Башня, расположенная в источник системы координат сценария, оснащена радарным датчиком. В $\mathit{t}$ = 0 секунд, грузовик в положении (10,20,0) метров едет в положительном положении $\mathit{X}$ направление со скоростью 5 м/с.

Радарный датчик выходов 3-D положение и измерения скорости в систему координат сценария, поэтому измерение может быть записано следующим образом:

measurement1 = [10;20;0;5;0;0]; % [x;y;z;vx;vy;vz]

Можно задать дополнительные свойства, такие как MeasurmentNoise, SensorIndex, ObjectClassID, и ObjectAttributes для objectDetection объект. Для примера, принимая стандартное отклонение шума измерения положения и скорости 10 м и 1 м/с, соответственно, можно задать матрицу ковариации ошибки измерения как:

measurementNoise1 = diag([10*ones(3,1);ones(3,1)]);

Создайте objectDetection использование этих значений.

time1 = 0; % detection time
detect1 = objectDetection(time1,measurement1,'MeasurementNoise',measurementNoise1)

detect1 = 
  objectDetection with properties:

                     Time: 0
              Measurement: [6x1 double]
         MeasurementNoise: [6x6 double]
              SensorIndex: 1
            ObjectClassID: 0
    MeasurementParameters: {}
         ObjectAttributes: {}

Пример 2: Преобразование обнаружений в движущейся прямоугольной системе координат

Рассмотрим 2-D сценарий отслеживания с ego-автомобилем и грузовиком. В $\mathit{t}$= 0 секунд, автомобиль расположен на (20,10,0) метрах относительно сценария системы координат. Машина движется со скоростью 5 м/с в $\mathit{Y}$ направление системы координат сценария. Локальная (передняя) система координат эго-автомобиля, {$\mathit{x}$,$\mathit{y}$}, поворачивается из системы координат сценария на угол 90 степеней. Как и в предыдущем примере, грузовик в положении (10,20,0) метров едет в положительном состоянии $\mathit{X}$ направление со скоростью 5 м/с.

Между тем эго-автомобиль наблюдает грузовик в собственной локальной системе координат, {$\mathit{x}$,$\mathit{y}$}. На практике измерение можно получить непосредственно из сенсорной системы эго-автомобиля. Из рисунка измерения грузовика составляют [10; 10; 0 -5; -5; 0] относительно {$\mathit{x}$,$\mathit{y}$} система координат в порядке [x;y;z;vx;vy;vz].

measurement2 = [10; 10; 0; -5;  -5; 0]; % [x;y;z;vx;vy;vz]

Чтобы задать обнаружение объектов, необходимо задать преобразование координат из прямоугольной системы координат сценария {$\mathit{X}$,$\mathit{Y}$} к локальной прямоугольной системе координат {$\mathit{x}$,$\mathit{y}$}. Можно использовать MeasurementParameters свойство objectDetection чтобы задать эти параметры преобразования. В преобразовании системы координат сценария является родительской системой координат, а локальная система координат ego-автомобиля - дочерней системой координат.

MP2 = struct();

MP2.Frame = 'rectangular'; 
MP2.OriginPosition =[20; 10; 0]; 
MP2.OriginVelocity = [0; 5; 0]; 

Задайте поворот с помощью матрицы поворота, преобразованной из углов Эйлера. Задайте IsParentToChild true, чтобы указать поворот родительской системы координат к дочерней системе координат.

rotAngle2 = [90 0 0]; % [yaw,pitch,row]
rotQuat2 = quaternion(rotAngle2,'Eulerd','ZYX','frame');
rotMatrix2 = rotmat(rotQuat2,'frame');
MP2.Orientation = rotMatrix2;
MP2.IsParentToChild = true; 

Задайте измерения.

MP2.HasElevation = false;
MP2.HasAzimuth = false;
MP2.HasRange = true;
MP2.HasVelocity = true;

Создайте objectDetection и задайте MeasurementParameters свойство.

time2 = 0;
detection2 = objectDetection(time2,measurement2,'MeasurementParameters',MP2)

detection2 = 
  objectDetection with properties:

                     Time: 0
              Measurement: [6x1 double]
         MeasurementNoise: [6x6 double]
              SensorIndex: 1
            ObjectClassID: 0
    MeasurementParameters: [1x1 struct]
         ObjectAttributes: {}

Чтобы проверить обнаружение объектов, можно использовать cvmeas функция измерения для регенерации измерения. The cvmeas функция может принять фактическое состояние цели и параметров измерения как вход. Входной вход состояния cvmeas находится в порядке [x;vx;y;vy;z;vz]. Как показано на следующем выходе, результаты согласуются с measurement2.

state2 =[10;5;20;0;0;0]; % [x;vx;y;vy;z;vz]
cvmeas2 = cvmeas(state2,MP2)% [x;y;z;vx;vy;vz]

cvmeas2 = 6×1

   10.0000
   10.0000
         0
   -5.0000
   -5.0000
         0

Пример 3: Преобразование обнаружений в движущейся сферической системе координат

Рассмотрим предыдущий сценарий отслеживания, только теперь измерение получает сканирующий радар со сферической выходной системой координат. Направление boresight радара выровнено с $\mathit{Y}$ направление (то же, что и $\mathit{x}$ направление) при $\mathit{t}$ = 0 секунд.

Поскольку относительная скорость между грузовиком и автомобилем находится в направлении линии видимости, измерение, которое находится в порядке [азимута; повышение; область значений; range-rate], может быть получено следующим образом:

measurement3 =[45; 0; 10/sind(45); -5/sind(45)]; % [az;el;rng;rr]. Units in degrees.

Задайте параметры измерения.

MP3 = struct();

MP3.Frame = 'spherical'; % The child frame is spherical.
MP3.OriginPosition = [20; 10; 0];
MP3.OriginVelocity = [0; 5; 0];

% Specify rotation.
rotAngle3 = [90 0 0];
rotQuat3 = quaternion(rotAngle3,'Eulerd','ZYX','frame');
rotMatrix3 = rotmat(rotQuat3,'frame');
MP3.Orientation = rotMatrix3;
MP3.IsParentToChild = true; 

Задайте HasElevation и HasAzimuth на true для вывода азимута и углов возвышения в сферической дочерней системе координат. Задайте HasRange и HasVelocity и то, и другое в true для выходного диапазона и уровня области значений, соответственно.

MP3.HasElevation = true;
MP3.HasAzimuth = true;
MP3.HasRange = true;
MP3.HasVelocity = true;

Создайте objectDetection объект.

time3 = 0;
detection3 = objectDetection(time3,measurement3,'MeasurementParameters',MP3)

detection3 = 
  objectDetection with properties:

                     Time: 0
              Measurement: [4x1 double]
         MeasurementNoise: [4x4 double]
              SensorIndex: 1
            ObjectClassID: 0
    MeasurementParameters: [1x1 struct]
         ObjectAttributes: {}

Проверьте результаты с помощью cvmeas. Результаты согласуются с measurement3.

state3 = [10;5;20;0;0;0]; % [x;vx;y;vy;z;vz]
cvmeas3 = cvmeas(state3,MP3) % [az;el;rng;rr]

cvmeas3 = 4×1

   45.0000
         0
   14.1421
   -7.0711

Пример 4: Преобразование обнаружений между тремя системами координат

Рассмотрим предыдущий сценарий слежения, только теперь направление boresight радара вращается на 45 степени от $\mathit{x}$ направление локальной системы координат автомобиля.

Новые измерения, выраженные в новой сферической системе координат {${\mathit{x}}^{\prime }$,${\mathit{y}}^{\prime }$являются:

measurement4 = [0; 0; 10/sind(45); -5/sind(45)]; % [az;el;rng;rr]

Для параметров измерения можно задать поворот как поворот на 135 градусов от системы координат сценария к новой сферической системе координат. Также можно задать его как два последовательных поворота: прямоугольный {$\mathit{X}$,$\mathit{Y}$} в прямоугольный {$\mathit{x}$,$\mathit{y}$} и прямоугольный {$\mathit{x}$,$\mathit{y}$} в сферический {${\mathit{x}}^{\prime }$,${\mathit{y}}^{\prime }$}. Чтобы проиллюстрировать функцию преобразования нескольких кадров, поддерживаемую MeasurementParameters свойство, этот пример использует последний подход.

Первый набор параметров измерения в точности аналогичен MP2 используется в примере 2. MP2 учитывает поворот из прямоугольной {$\mathit{X}$,$\mathit{Y}$} к прямоугольной {$\mathit{x}$,$\mathit{y}$}. Для второго набора параметров измерения, MP4, вам нужно задать только 45-градусный поворот от прямоугольной {$\mathit{x}$,$\mathit{y}$} в сферический {${\mathit{x}}^{\prime }$,${\mathit{y}}^{\prime }$}.

MP4 = struct();

MP4.Frame = 'spherical';
MP4.OriginPosition =[0; 0; 0]; % Colocated positions.
MP4.OriginVelocity = [0; 0; 0]; % Same origin velocities.

% Specify rotation.
rotAngle4 = [45 0 0];
rotQuat4 = quaternion(rotAngle4,'Eulerd','ZYX','frame');
rotMatrix4 = rotmat(rotQuat4,'frame');
MP4.Orientation = rotMatrix4;
MP4.IsParentToChild = true; 

% Specify outputs in the spherical child frame.
MP4.HasElevation = true;
MP4.HasAzimuth = true;
MP4.HasRange = true;
MP4.HasVelocity = true;

Создайте объединенную MeasurementParameters вход, MPc.

MPc =[MP4 MP2];

Создайте objectDetection объект.

time4 = 0;
detection4 = objectDetection(time4,measurement4,'MeasurementParameters',MPc)

detection4 = 
  objectDetection with properties:

                     Time: 0
              Measurement: [4x1 double]
         MeasurementNoise: [4x4 double]
              SensorIndex: 1
            ObjectClassID: 0
    MeasurementParameters: [1x2 struct]
         ObjectAttributes: {}

Проверьте результаты с помощью cvmeas. Результат согласен с measurement4.

state4 = [10;5;20;0;0;0]; % [x;vx;y;vy;z;vz]
cvmeas4 = cvmeas(state4,MPc) % [az;el;rr;rrate]

cvmeas4 = 4×1

    0.0000
         0
   14.1421
   -7.0711

Пример 5: Преобразование обнаружений 3D

Рассмотрим беспилотное летательное транспортное средство (БПЛА), осуществляющий мониторинг области. В $\mathit{t}$ = 0 секунд, БПЛА находится в положении (5,5, -1) км относительно глобальной системы координат северо-востока (NED). Скорость БПЛА составляет (-50, -100,5) м/с. Ориентация БПЛА каркаса кузова {$\mathit{x}$,$\mathit{y}$,$\mathit{z}$} относительно глобальной системы координат NED задается как (-120,2,2) степени в рыскании, тангаже и крене. При этом машина на положении (1,1,0) км движется на восток со скоростью 30 м/с. БПЛА измеряет автомобиль с помощью радиолокационной системы, выровненной по оси собственного кузова.

На основе этой информации задайте кинематические параметры для преобразования измерения.

Задайте тип кадра, источника положение и источник скорость каркаса кузова БПЛА.

MP5 = struct();

MP5.Frame = 'spherical'; 
MP5.OriginPosition = [5000; 5000; -1000]; 
MP5.OriginVelocity = [-50; -100; 5]; 

Задайте поворот от системы координат NED к каркасу кузова БПЛА.

Rot_angle5 = [-120 2 2]; % [yaw,pitch,roll]
Rot_quat5 = quaternion(Rot_angle5,'Eulerd','ZYX','frame');
Rot_matrix5 = rotmat(Rot_quat5,'frame');
MP5.Orientation = Rot_matrix5;
MP5.IsParentToChild = true; 

Задайте выходные измерения в сферической системе координат.

MP5.HasElevation = true;
MP5.HasAzimuth = true;
MP5.HasRange = true;
MP5.HasVelocity = true;

Измерение можно получить непосредственно из радиолокационной системы на БПЛА. Используйте cvmeas функция состоит в том, чтобы получить измерение. Измерение находится в порядке [азимут; повышение; диапазон; частота области значений].

car_state5 = [1000;0;1000;30;0;0]; % [x;vx;y;vy;z;vz].
measurement5 = cvmeas(car_state5,MP5);
meas_az5 = measurement5(1)

meas_az5 = -14.6825

meas_el5 = measurement5(2)

meas_el5 = 12.4704

meas_rng5 = measurement5(3)

meas_rng5 = 5.7446e+03

meas_rr5 = measurement5(4)

meas_rr5 = -126.2063

Угол возвышения определяется как угол от плоскости xy до направления z. Именно поэтому угол возвышения положителен для цели на земле относительно БПЛА. Это соглашение используется во всем тулбоксе.

Шум измерения для азимута, повышения, диапазона и скорости области значений составляет [1,1,20,2], соответственно. Кроме того, индекс радара равен 2, и радар может классифицировать обнаруженный объект как 1 для типа 'car'.

index5 = 2; 
covariance5 = diag([1;1;20;2]);
classID5 = 1;

Создайте objectDetection объект для обнаружения.

time5 = 0;
detection = objectDetection(time5,measurement5,'SensorIndex',index5,...
            'MeasurementNoise',covariance5,'ObjectClassID',classID5,'MeasurementParameters',MP5)

detection = 
  objectDetection with properties:

                     Time: 0
              Measurement: [4x1 double]
         MeasurementNoise: [4x4 double]
              SensorIndex: 2
            ObjectClassID: 1
    MeasurementParameters: [1x1 struct]
         ObjectAttributes: {}