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

Введение

Тулбокс имеет три блока Simulink для нелинейной оценки состояния:

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

Этот пример использует блок Extended Kalman Filter, чтобы продемонстрировать первые два шага этого рабочего процесса. Последние два шага кратко обсуждены в разделе Next Steps. Цель в этом примере состоит в том, чтобы оценить состояния объекта с помощью шумных измерений, обеспеченных радаром и датчиком GPS. Состояния объекта являются его положением и скоростью, которые обозначаются как xTrue в модели Simulink.

Если вы интересуетесь блоком Particle Filter, смотрите пример "Параметр и Оценка состояния в Simulink Используя Блок Фильтра частиц".

addpath(fullfile(matlabroot,'examples','control','main')) % add example data
open_system('multirateEKFExample');

Моделирование объекта

Алгоритм расширенного фильтра Калмана (EKF) требует функции изменения состояния, которая описывает эволюцию состояний от одного временного шага до следующего. Блок поддерживает следующие две функциональных формы:

Здесь f (..) функция изменения состояния, x является состоянием, и w является шумом процесса. u является дополнительным, и представляет дополнительные входные параметры f, например, системные входные параметры или параметры. Аддитивный шум означает, что следующее состояние и шум процесса связаны линейно. Если отношение нелинейно, используйте неаддитивную форму.

Функцией f (...) может быть функция MATLAB, которые выполняют ограничения MATLAB Coder™ или Функциональный блок Simulink. После того, как вы создаете f (...), вы задаете имя функции и является ли шум процесса дополнением или недополнением в блоке Extended Kalman Filter.

В этом примере вы отслеживаете северные и восточные положения и скорости объекта на 2-мерной плоскости. Предполагаемые количества:

Вот индекс дискретного времени. Используемое уравнение изменения состояния имеет неаддитивную форму, где вектор состояния и является шумом процесса. Фильтр принимает, что это - нулевое среднее значение, независимая случайная переменная с известным отклонением. A и матрицы G:

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

В этом примере вы реализуете функцию изменения состояния использование Функционального блока Simulink. Для этого

Аналитический якобиан функции изменения состояния также реализован в этом примере. Определение якобиана является дополнительным. Однако это уменьшает вычислительную нагрузку, и в большинстве случаев увеличивает точность оценки состояния. Реализуйте Функцию Якоби как функцию Simulink, потому что функция изменения состояния является функцией Simulink.

open_system('multirateEKFExample/Simulink Function - State Transition Jacobian');

Моделирование датчика - Радар

Блоку Extended Kalman Filter также нужна функция измерения, которая описывает, как состояния связаны с измерениями. Следующие две функциональных формы поддерживаются:

Здесь h (..) функция измерения, и v является шумом измерения. u является дополнительным, и представляет дополнительные входные параметры h, например, системные входные параметры или параметры. Эти входные параметры могут отличаться от входных параметров в функции изменения состояния.

В этом примере радар, расположенный в начале координат, измеряет область значений и угол объекта на уровне 20 Гц. Примите, что оба из измерений имеют приблизительно 5%-й шум. Это может быть смоделировано следующим уравнением измерения:

Здесь и условия шума измерения, каждый с отклонением 0.05^2. Таким образом, большинство измерений имеет ошибки меньше чем 5%. Шум измерения является недополнением, потому что и просто не добавляются к измерениям, но вместо этого они зависят от состояний x. В этом примере радарное уравнение измерения реализовано с помощью Функционального блока Simulink.

open_system('multirateEKFExample/Simulink Function - Radar Measurements');

Моделирование датчика - GPS

GPS измеряет восточные и северные положения объекта на уровне 1 Гц. Следовательно, уравнение измерения для датчика GPS:

Здесь и условия шума измерения с ковариационной матрицей [10^2 0; 0 10^2]. Таким образом, измерения точны приблизительно до 10 метров, и ошибки являются некоррелироваными. Шум измерения является дополнением, потому что шумовые условия влияют на измерения линейно.

Создайте эту функцию и сохраните ее в файле с именем gpsMeasurementFcn.m. Когда шум измерения является дополнением, вы не должны задавать шумовые условия в функции. Вы обеспечиваете это имя функции и ковариацию шума измерения в блоке Extended Kalman Filter.

type gpsMeasurementFcn

function y = gpsMeasurementFcn(x)
% gpsMeasurementFcn GPS measurement function for state estimation
%
% Assume the states x are:
%   [EastPosition; NorthPosition; EastVelocity; NorthVelocity]

%#codegen

% The %#codegen tag above is needed is you would like to use MATLAB Coder to 
% generate C or C++ code for your filter

y = x([1 2]); % Position states are measured
end

Отфильтруйте конструкцию

Сконфигурируйте блок Extended Kalman Filter, чтобы выполнить оценку. Вы задаете имена функций изменения состояния и измерения, начальное состояние и утверждаете ошибочную ковариацию и характеристики шума процесса и измерения.

Во вкладке System Model диалогового окна блока задайте следующие параметры:

Изменение состояния

Инициализация

С тех пор существует два датчика, нажимают Add Measurement, чтобы задать вторую функцию измерения.

Измерение 1

Измерение 2

Во вкладке Multirate, поскольку эти два датчика действуют на уровне различных частот дискретизации, выполняют следующую настройку:

Симуляция и результаты

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

Сгенерируйте истинные значения состояния, соответствующие этому движению:

Ts = 0.05; % [s] Sample rate for the true states
[t, xTrue] = generateTrueStates(Ts); % Generate position and velocity profile over 0-185 seconds

Симулируйте модель. Например, посмотрите на фактические и предполагаемые скорости в восточном направлении:

sim('multirateEKFExample');
open_system('multirateEKFExample/Scope - East Velocity');

График показывает истинную скорость в восточном направлении и его расширенные оценки Фильтра Калмана. Фильтр успешно отслеживает изменения в скорости. Многоскоростная природа фильтра является самой очевидной в области значений времени t = 20 - 30 секунд. Фильтр делает большие коррекции каждую секунду (частота дискретизации GPS), в то время как коррекции из-за радарных измерений отображаются каждые 0.05 секунды.

Следующие шаги

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

Этот пример показал, как использовать блок Extended Kalman Filter в System Identification Toolbox. Вы оценили положение и скорость объекта от двух различных датчиков, действующих на различных уровнях выборки.

close_system('multirateEKFExample', 0);