trackingKF

Линейный фильтр Калмана для отслеживания объектов

развернуть все на странице

Описание

A trackingKF объект является линейным фильтром Калмана в дискретном времени, используемым для отслеживания положений и скоростей объектов, с которыми можно столкнуться в сценарии беспилотного управления. К таким объектам относятся автомобили, пешеходы, велосипеды и стационарные конструкции или препятствия.

Фильтр Калмана является рекурсивным алгоритмом для оценки развивающегося состояния процесса, когда измерения производятся на процессе. Фильтр линейен, когда эволюция состояния следует линейной модели движения, и измерения являются линейными функциями состояния. Фильтр принимает, что и процесс, и измерения имеют аддитивный шум. Когда шум процесса и шум измерения Гауссовы, фильтр Калмана является оптимальным средством оценки состояния минимальной средней квадратичной невязки (MMSE) для линейных процессов.

Использовать этот объект можно следующими способами:

  • Явным образом установите модель движения. Установите свойство модели движения, MotionModel, в Custom, а затем используйте StateTransitionModel свойство для установки матрицы переходов состояний.

  • Установите MotionModel свойство предопределенной модели перехода состояний:

    Модель движения
    '1D Constant Velocity'
    '1D Constant Acceleration'
    '2D Constant Velocity'
    '2D Constant Acceleration'
    '3D Constant Velocity'
    '3D Constant Acceleration'

Создание

Синтаксис

filter = trackingKF
filter = trackingKF(F,H)
filter = trackingKF(F,H,G)
filter = trackingKF('MotionModel',model)
filter = trackingKF(___,Name,Value)

Описание

filter = trackingKF создает линейный объект фильтра Калмана для движущегося объекта в дискретном времени, 2-D, постоянной скорости. Фильтр Калмана использует значения по умолчанию для StateTransitionModel, MeasurementModel, и ControlModel свойства. Функция также устанавливает MotionModel свойство к '2D Constant Velocity'.

filter = trackingKF(F,H) задает модель перехода состояния, F, и модель измерения, H. С помощью этого синтаксиса функция также устанавливает MotionModel свойство к 'Custom'.

filter = trackingKF(F,H,G) также задает модель управления, G. С помощью этого синтаксиса функция также устанавливает MotionModel свойство к 'Custom'.

filter = trackingKF('MotionModel',model) устанавливает свойство модели движения, MotionModel, в model.

пример

filter = trackingKF(___,Name,Value) конфигурирует свойства фильтра Калмана при помощи одной или нескольких Name, Value аргументы в виде пар и любой из предыдущих синтаксисов. Любые неопределенные свойства берут значения по умолчанию.

Свойства

расширить все

Состояние фильтра Калмана, заданное как действительный вектор M -элемент. M - размер вектора состояния. Типичные размеры вектора состояний описаны в MotionModel свойство. Когда начальное состояние задано как скаляр, состояние расширяется в вектор M -элемент.

Можно задать состояние скаляром в следующих случаях:

  • Когда MotionModel для свойства задано значение 'Custom', M определяется размером модели перехода состояния.

  • Когда MotionModel для свойства задано значение '2D Constant Velocity', '3D Constant Velocity', '2D Constant Acceleration', или '3D Constant Acceleration'необходимо сначала задать состояние как M элемент. Можно использовать скаляр для всех последующих спецификаций вектора состояния.

Пример: [200;0.2;-40;-0.01]

Типы данных: double

Ковариация ошибки состояния, заданная как положительный скаляр или положительно-определенная матрица M -by- M, где M является размером состояния. Установка значения в качестве скаляра создает произведение единичной матрицы M -by M. Эта матрица представляет неопределенность в состоянии.

Пример: [20 0.1; 0.1 1]

Типы данных: double

Модель движения фильтра Калмана, заданная как 'Custom' или одну из этих предопределенных моделей. В этом случае вектор состояния и матрица переходов принимают форму, указанную в таблице.

Модель движенияФорма вектора состоянийФорма модели перехода состояния
'1D Constant Velocity'

[x;vx]

[1 dt; 0 1]

'2D Constant Velocity'

[x;vx;y;vy]

Блочная диагональная матрица с [1 dt; 0 1] блок, повторенный для x и y пространственных размерностей

'3D Constant Velocity'

[x;vx;y;vy;z;vz]

Блочная диагональная матрица с [1 dt; 0 1] блок, повторенный для x, y и z пространственных размерностей.

'1D Constant Acceleration'

[x;vx;ax]

[1 dt 0.5*dt^2; 0 1 dt; 0 0 1]

'2D Constant Acceleration'

[x;vx;ax;y;vy;ay]

Блочная диагональная матрица с [1 dt 0.5*dt^2; 0 1 dt; 0 0 1] блоки, повторенные для x и y пространственных размерностей

'3D Constant Acceleration'

[x;vx,ax;y;vy;ay;z;vz;az]

Блочная диагональная матрица с [1 dt 0.5*dt^2; 0 1 dt; 0 0 1] блок, повторенный для x, y и z пространственных размерностей

Когда ControlModel задано свойство, каждый ненулевой элемент модели перехода состояния заменяется dt.

Когда MotionModel является 'Custom'необходимо задать матрицу модели перехода состояний, матрицу модели измерения и, опционально, матрицу модели управления в качестве входных параметров в фильтр Калмана.

Типы данных: char

Модель перехода состояния между временными шагами, заданная как матрица вещественного M -by M. M - размер вектора состояния. В отсутствие средств управления и шума модель перехода состояния связывает состояние в любой временной шаг с состоянием на предыдущем шаге. Модель перехода состояния является функцией от размера временного шага фильтра.

Пример: [1 0; 1 2]

Зависимости

Чтобы включить это свойство, задайте MotionModel на 'Custom'.

Типы данных: double

Модель управления, заданная как M -by - L матрица. M - размерность вектора состояний, а L - количество элементов управления или сил. Модель управления добавляет эффект управлений на эволюцию состояния.

Пример: [.01 0.2]

Типы данных: double

Ковариация технологического шума, заданная как положительный скаляр или M -by - M матрица, где M - размерность состояния. Если вы задаете это свойство как скаляр, фильтр использует значение как умножитель единичной матрицы M -by M. Process шум выражает неопределенность в динамической модели и принимается как нулевой Гауссов белый шум.

Совет

Если вы задаете MotionModel свойство как любая из предопределенной модели движения, тогда соответствующий технологический шум автоматически генерируется во время конструкции и обновляется во время распространения. В этом случае вам не нужно указывать ProcessNoise свойство. Фактически, фильтр пренебрегает шумом вашего процесса, входа во время конструкции объекта. Если вы хотите задать технологический шум, отличный от значений по умолчанию, используйте trackingEKF объект.

Типы данных: double

Модель измерения из вектора состояния, заданная как вещественная N -by - M матрица, где N - размер вектора измерения, а M - размер вектора состояния. measurement model является линейной матрицей, которая определяет предсказанные измерения из предсказанного состояния.

Пример: [1 0.5 0.01; 1.0 1 0]

Типы данных: double

Ковариация шума измерения, заданная как положительный скаляр или положительно-определенная, действительная N -by - N матрица, где N - размер вектора измерения. Если вы задаете это свойство как скаляр, фильтр использует значение как умножитель единичной матрицы N -by N. Шум измерения представляет неопределенность измерения и принимается как нулевой Гауссов белый шум.

Пример: 0.2

Типы данных: double

Включите сглаживание состояния, заданное как false или true. Установка значения свойства true требуется лицензия Sensor Fusion and Tracking Toolbox™. Если задано как true, можно:

  • Используйте smooth (Sensor Fusion and Tracking Toolbox), предусмотренная в Sensor Fusion and Tracking Toolbox, для сглаживания оценок состояния на предыдущих шагах. Внутри фильтр сохраняет результаты предыдущих шагов, чтобы позволить сглаживание назад.

  • Задайте максимальное количество шагов сглаживания с помощью MaxNumSmoothingSteps свойство отслеживающего фильтра.

Максимальное количество шагов сглаживания назад, заданное как положительное целое число.

Зависимости

Чтобы включить это свойство, установите EnableSmoothing свойство к true.

Функции объекта

predictПредсказать состояние и ковариацию ошибки расчета состояния линейного фильтра Калмана
correctПравильное состояние и ковариация ошибки оценки состояния с помощью отслеживающего фильтра
correctjpdaПравильное состояние и ковариация ошибки оценки состояния с помощью отслеживающего фильтра и JPDA
distanceРасстояния между током и предсказанными измерениями отслеживающего фильтра
likelihoodВероятность измерения с отслеживающего фильтра
cloneСоздайте повторяющийся фильтр отслеживания
residualИзмерение остаточного и остаточного шума от отслеживающего фильтра
initializeИнициализируйте состояние и ковариацию отслеживающего фильтра

Примеры

свернуть все

Открыть Live Script

Создайте линейный фильтр Калмана, который использует 2D Constant Velocity модель движения. Предположим, что измерение состоит из местоположения x-y объекта.

Задайте начальную оценку состояния, которая будет иметь нулевую скорость.

x = 5.3;
y = 3.6;
initialState = [x;0;y;0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);

Создайте измеренные положения из траектории постоянной скорости.

vx = 0.2;
vy = 0.1;
T  = 0.5;
pos = [0:vx*T:2;5:vy*T:6]';

Спрогнозируйте и исправьте состояние объекта.

for k = 1:size(pos,1)
    pstates(k,:) = predict(KF,T);
    cstates(k,:) = correct(KF,pos(k,:));
end

Постройте графики дорожек.

plot(pos(:,1),pos(:,2),'k.', pstates(:,1),pstates(:,3),'+', ...
    cstates(:,1),cstates(:,3),'o')
xlabel('x [m]')
ylabel('y [m]')
grid
xt  = [x-2 pos(1,1)+0.1 pos(end,1)+0.1];
yt = [y pos(1,2) pos(end,2)];
text(xt,yt,{'First measurement','First position','Last position'})
legend('Object position', 'Predicted position', 'Corrected position')

Figure contains an axes. The axes contains 6 objects of type line, text. These objects represent Object position, Predicted position, Corrected position.

Подробнее о

расширить все

Алгоритмы

Фильтр Калмана описывает движение объекта путем оценки его состояния. Состояние обычно состоит из положения объекта и скорости и, возможно, его ускорения. Состояние может охватывать одно, два или три пространственных размерности. Чаще всего вы используете фильтр Калмана, чтобы смоделировать движение с постоянной скоростью или с постоянным ускорением. Линейный фильтр Калмана принимает, что процесс подчиняется следующему линейному стохастическому разностному уравнению:

xk+1=Fkxk+Gkuk+vk

xk - состояние на шаге k. Fk - матрица модели перехода состояний. Gk - матрица модели управления. uk представляет известные обобщенные элементы управления, действующие на объект. В дополнение к заданным уравнениям движения на движение могут влиять случайные шумовые возмущения, vk. Состояние, матрица переходов и элементы управления вместе предоставляют достаточно информации, чтобы определить будущее движение объекта в отсутствие шума.

В фильтре Калмана измерения также являются линейными функциями состояния,

zk=Hkxk+wk

где Hk - матрица модели измерения. Эта модель выражает измерения как функции состояния. Измерение может состоять из положения, положения и скорости объекта, или его положения, скорости и ускорения, или некоторой функции этих величин. Измерения могут также включать шумовые возмущения, wk.

Эти уравнения, в отсутствие шума, моделируют фактическое движение объекта и фактические измерения. Шумовые вклады на каждом шаге неизвестны и не могут быть смоделированы. Известны только шумовые ковариационные матрицы. Ковариационная матрица состояния обновляется знанием только о шумовой ковариации.

Краткое описание алгоритма линейного фильтра Калмана смотрите в Linear Kalman Filters.

Ссылки

[1] Коричневый, R.G. и P.Y.C. Ван. Введение в анализ случайных сигналов и прикладную фильтрацию Калмана. 3-е издание. Нью-Йорк: John Wiley & Sons, 1997.

[2] Калман, Р. Е. «Новый подход к линейным задачам фильтрации и предсказания». Сделка ASME-Journal of Basic Engineering, том 82, серия D, март 1960, стр. 35-45.

[3] Блэкман, Сэмюэль. Отслеживание нескольких целей с помощью радаров. Дом Артеха. 1986.

Расширенные возможности

.

См. также

Функции

Объекты

Темы

Введенный в R2017a

Документация по Automated Driving Toolbox

Поддержка

Для просмотра документации необходимо авторизоваться на сайте