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

Введение

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

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

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

open_system('ctrlKalmanNavigationExample');

Модель автомобиля

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

где состояния автомобиля:

параметры автомобиля:

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

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

Модель автомобиля реализована в подсистеме ctrlKalmanNavigationExample/Vehicle Model. Модель Simulink содержит два контроллера PI для отслеживания желаемой ориентации и скорости для автомобиля в подсистеме 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' находится в библиотеке System Identification Toolbox/Estimators в Simulink. Это находится также в библиотеке Control System Toolbox. Сконфигурируйте параметры блоков для оценки состояния дискретного времени. Задайте следующие параметры Настроек Фильтра:

Кликните по вкладке 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');