Документация

Введение

Вы хотите оценить положение и скорость наземного транспортного средства в северных и восточных направлениях. Транспортное средство может переместиться свободно в двумерное пространство без любых ограничений. Вы проектируете многоцелевую навигацию и систему слежения, которая может использоваться в любом объекте и не только транспортном средстве.

и восточные и северные положения транспортного средства от источника, ориентация транспортного средства с востока и держащийся угол транспортного средства. переменная непрерывного времени.

Модель Simulink состоит из двух основных частей: модель Vehicle и Фильтр Калмана. Они объяснены далее в следующих разделах.

open_system('ctrlKalmanNavigationExample');

Модель транспортного средства

Гусеничная машина представлена с простой массовой точкой моделью:

где состояния транспортного средства:

параметры транспортного средства:

и входные параметры управления:

Продольные движущие силы модели игнорируют сопротивление качению шины. Боковые движущие силы модели принимают, что желаемый руководящий угол может быть достигнут мгновенно и проигнорировать момент отклонения от курса инерции.

Модель автомобиля реализована в ctrlKalmanNavigationExample/Vehicle Model подсистема. Модель Simulink содержит два ПИ-контроллера для отслеживания желаемой ориентации и скорости для автомобиля в ctrlKalmanNavigationExample/Speed And Orientation Tracking подсистема. Это позволяет вам задавать различные условия работы для автомобиля и проверять производительность Фильтра Калмана.

Проект фильтра Калмана

Фильтр Калмана является алгоритмом, чтобы оценить неизвестные переменные интереса на основе линейной модели. Эта линейная модель описывает эволюцию предполагаемых переменных в зависимости от времени в ответ на начальные условия модели, а также известные и неизвестные входные параметры модели. В этом примере вы оцениваете следующие параметры/переменные:

где

Термины обозначают скорости а не оператор производной. индекс дискретного времени. Модель, используемая в Фильтре Калмана, имеет форму:

где вектор состояния, является измерениями, является шумом процесса и является шумом измерения. Фильтр Калмана принимает, что и нулевые средние, независимые случайные переменные с известными отклонениями, и. Здесь, A, G, и матрицы C:

где

Третья строка модели A и G восточная скорость как случайный обход:. в действительности положение является переменной непрерывного времени и является интегралом скорости в зависимости от времени. Первая строка A и G представляет дискретное приближение этому кинематическому отношению:. вторые и четвертые строки A и G представляют то же отношение между северной скоростью и положением.

Матрица C представляет то единственное положение, измерения доступны. Датчик положения, такой как GPS, обеспечивает эти измерения на уровне частоты дискретизации 1 Гц. Отклонение шума измерения, матрицы R, задано как. Поскольку R задан как скаляр, блок Фильтра Калмана принимает, что матрица R является диагональной, ее диагонали равняются 50, и имеет совместимые размерности с y. Если шум измерения является Гауссовым, R=50 соответствует 68% измерений положения, являющихся в или фактического положения в восточных и северных направлениях. Однако это предположение не необходимо для Фильтра Калмана.

Элементы получения, насколько скорость транспортного средства может переключить один шаг расчета Ts. Отклонение шума процесса w, матрицы Q, выбрано, чтобы быть изменяющимся во времени. Это получает интуицию, что типичные значения меньше, когда скорость является большой. Например, движение от 0 до 10m/s легче, чем движение от 10 до 20m/s. Конкретно вы используете предполагаемые северные и восточные скорости и функцию насыщения, чтобы создать Q [n]:

Диагонали модели Q отклонение w, обратно пропорционального квадрату предполагаемых скоростей. Функция насыщения препятствует тому, чтобы Q стал слишком большим или маленьким. Коэффициент 250 получен от метода наименьших квадратов до 0-5, 5-10, 10-15, 15-20, 20-25m/s ускоряющие данные времени для типового транспортного средства. Обратите внимание на то, что диагональ Q выбор представляет наивное предположение, что изменения скорости в северном и восточном направлении являются некоррелироваными.

Входные параметры блока фильтра Калмана и Setup

Блок 'Kalman Filter' находится в Control System Toolbox библиотека в Simulink. Это находится также в System Identification Toolbox/Estimators библиотека. Сконфигурируйте параметры блоков для оценки состояния дискретного времени. Задайте следующие параметры Настроек Фильтра:

Кликните по вкладке Options, чтобы установить импорт блока и опции выходного порта:

Нажмите Model Parameters, чтобы задать модель объекта управления и шумовые характеристики:

Результаты

Проверьте производительность Фильтра Калмана путем симуляции сценария, где транспортное средство делает следующие маневры:

Симулируйте модель Simulink. Постройте фактическое, измеренный и оценки Фильтра Калмана положения транспортного средства.

sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',...
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;

Ошибка между измеренным и фактическим положением, а также ошибка между оценкой Фильтра Калмана и фактическим положением:

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;

Легенды графика показывают ошибку измерения и оценки положения (и) нормированный количеством точек данных. Оценки Фильтра Калмана имеют приблизительно 25%-й процент меньше ошибки, чем необработанные измерения.

Фактическую скорость в восточном направлении и его оценке Фильтра Калмана показывают ниже в главном графике. Нижний график показывает ошибку оценки.

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;

Легенда на диаграмме погрешностей показывает восточную ошибку оценки скорости, нормированную количеством точек данных.

Оценки Фильтра Калмана скорости отслеживают фактические скоростные тренды правильно. Уровень шума уменьшается, когда транспортное средство перемещается при высоких скоростях. Это соответствует проекту матрицы Q. Большие два скачка в t=50s и t=200s. Это времена, когда автомобиль проходит внезапное замедление и крутой поворот, соответственно. Изменения скорости в те моменты намного больше, чем прогнозы от Фильтра Калмана, который основан на его матричном входе Q. После нескольких тактов оценки фильтра догоняют фактическую скорость.

Сводные данные

Вы оценили положение и скорость транспортного средства с помощью блока Фильтра Калмана в Simulink. Движущие силы шума процесса модели были изменяющимися во времени. Вы подтвердили производительность фильтра путем симуляции различных маневров транспортного средства и случайным образом сгенерировали шум измерения. Фильтр Калмана улучшил измерения положения и обеспечил оценки скорости для транспортного средства.

bdclose('ctrlKalmanNavigationExample');