Этот пример показывает, как выполнить нелинейную оценку состояния в Simulink™ для системы с несколькими датчиками, работающими с различными скоростями дискретизации. Блок Extended Фильтра Калмана в System Identification Toolbox™ используется, чтобы оценить положение и скорость объекта с помощью GPS и радиолокационных измерений.
Тулбокс имеет три блока Simulink для нелинейной оценки состояния:
Расширенный фильтр Калмана: Реализует алгоритм расширенного фильтра Калмана первого порядка в дискретном времени.
Сигма-точечный фильтр Калмана: Реализует дискретный алгоритм сигма-точечного фильтра Калмана.
Фильтр частиц: Реализует алгоритм фильтра частиц в дискретном времени.
Эти блоки поддерживают оценку состояния с помощью нескольких датчиков, работающих с различными скоростями дискретизации. Типичный рабочий процесс использования этих блоков следующий:
Моделируйте поведение своего объекта и датчика с помощью функций MATLAB или Simulink.
Сконфигурируйте параметры блока.
Симулируйте фильтр и анализируйте результаты, чтобы получить доверие в эффективности фильтра.
Разверните фильтр на своем оборудовании. Вы можете сгенерировать код для этих фильтров с помощью программного обеспечения Simulink Coder™.
Этот пример использует блок Extended Kalman Filter, чтобы продемонстрировать первые два шага этого рабочего процесса. Последние два шага кратко рассматриваются в разделе «Следующие шаги». Цель в этом примере состоит в том, чтобы оценить состояния объекта с помощью шумных измерений, предоставляемых радаром и GPS-датчиком. Состояниями объекта являются его положение и скорость, которые обозначаются как xTrue в модели Simulink.
Если вы заинтересованы в блоке Particle Filter, смотрите пример «Оценка параметра и состояния в Simulink с использованием блока фильтра частицы».
addpath(fullfile(matlabroot,'examples','ident','main')) % add example data open_system('multirateEKFExample');
Расширенный алгоритм фильтра Калмана (EKF) требует функции перехода состояния, которая описывает эволюцию состояний от одного временного шага к следующему. Блок поддерживает следующие две формы функций:
Аддитивный технологический шум:
Неаддитивный шум процесса:
Здесь f (..) - переходная функция состояния, x - состояние, а w - шум процесса. u является необязательным и представляет дополнительные входы f для образца входов или параметров системы. Аддитивный шум означает, что следующее состояние и технологический шум связаны линейно. Если отношение нелинейно, используйте неаддитивную форму.
Функция f (...) может быть функцией MATLAB, которая соответствует ограничениям MATLAB Coder™, или блоком Simulink Function. После того, как вы создали f (...), вы задаете имя функции и является ли технологический шум аддитивным или неаддитивным в блоке Extended Kalman Filter.
В этом примере вы отслеживаете северные и восточные положения и скорости объекта на 2-мерной плоскости. Предполагаемые количества:
Вот индекс в дискретном времени. Используемое уравнение перехода состояния имеет неаддитивную форму, где является вектором состояния и является шумом процесса. Фильтр принимает, что является независимой от нуля случайной переменной с известным отклонением. Матрицы A и G:
где - шаг расчета. Третья строка A и G моделируют восточную скорость как случайную прогулку:. В действительности положение является переменной непрерывного времени и является интегралом скорости с течением времени. Первая строка A и G представляет дискретное приближение к этому кинематическому отношению:. Вторая и четвертая строки A и G представляют одинаковое соотношение между скоростью и положением на севере. Эта модель перехода состояний линейна, но модель радиолокационного измерения нелинейна. Эта нелинейность требует использования нелинейного оценщика состояния, такого как расширенный фильтр Калмана.
В этом примере вы реализуете функцию перехода состояния с помощью блока Simulink Function. Для этого,
Добавление Simulink Function
блокируйте в свою модель из Simulink/User-Defined Functions
библиотека
Щелкните имя, показанное на блоке Simulink Function. Отредактируйте имя функции и при необходимости добавьте или удалите входные и выходные аргументы. В этом примере имя для функции перехода состояния stateTransitionFcn
. Он имеет один выходной аргумент (xNext) и два входных параметров (x, w).
Хотя это не требуется в этом примере, можно использовать любые сигналы от остальной части вашей модели Simulink в функции Simulink. Для этого добавьте Inport
блоки от Simulink/Sources
библиотека. Обратите внимание, что они отличаются от ArgIn
и ArgOut
блоки, которые устанавливаются через сигнатуру вашей функции (xNext = stateTransitionFcn (x, w)).
В блоке Simulink Function создайте свою функцию, используя блоки Simulink.
Установите размерности для входного и выходных аргументов x, w и xNext на вкладке Signal Attributes ArgIn
и ArgOut
блоки. Тип данных и размерности портов должны соответствовать информации, которую вы предоставляете в Extended Kalman Filter
блок.
В этом примере также реализован аналитический якобиан функции перехода состояния. Установка значения якобиана опциональна. Однако это уменьшает вычислительную нагрузку и в большинстве случаев увеличивает точность оценки состояния. Реализуйте функцию Якобяна как функцию Simulink, потому что функция перехода состояния является функцией Simulink.
open_system('multirateEKFExample/Simulink Function - State Transition Jacobian');
Блок Расширенного Фильтра Калмана также нуждается в функции измерения, которая описывает, как состояния связаны с измерениями. Поддерживаются следующие две формы функций:
Аддитивный шум измерения:
Неаддитивный шум измерения:
Здесь h (..) - функция измерения, а v - шум измерения. u является необязательным и представляет дополнительные входы h для образца входов или параметров системы. Эти входы могут отличаться от входов в функции перехода состояния.
В этом примере радар, расположенный в источник, измеряет область значений и угол объекта на 20 Гц. Предположим, что оба измерения имеют около 5% шума. Это может быть смоделировано следующим уравнением измерения:
Вот и являются терминами шума измерения, каждый с отклонением 0,05 ^ 2. То есть большинство измерений имеют ошибки менее 5%. Шум измерения является неаддитивным, потому что и не просто добавляются к измерениям, но вместо этого они зависят от состояний x. В этом примере уравнение радиолокационного измерения реализуется с помощью блока Simulink Function.
open_system('multirateEKFExample/Simulink Function - Radar Measurements');
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, чтобы выполнить оценку. Вы задаете имена функции перехода и измерения состояния, начальное состояние и ковариацию ошибки состояния, а также характеристики шума процесса и измерения.
На вкладке Системная модель диалогового окна блоков задайте следующие параметры:
Переход между состояниями
Задайте функцию перехода между состояниями, stateTransitionFcn
, в Function. Поскольку у вас есть якобиан этой функции, выберите якобиан и задайте якобиан функции, stateTransitionJacobianFcn
.
Выберите Nonadditive
в раскрывающемся списке Process Noise, потому что вы явным образом заявили, как шум процесса влияет на состояния в вашей функции.
Задайте ковариацию шума процесса как [0,2 0; 0 0.2]. Как объяснено в разделе Моделирование этого примера, условия технологического шума определяют случайную прогулку скоростей в каждом направлении. Диагональные условия приблизительно фиксируют, насколько скорости могут измениться в течение одного шага расчета функции перехода состояния. Диагональные условия установлены в нуль, что является наивным предположением, что изменения скорости в северном и восточном направлениях являются некоррелированными.
Инициализация
Укажите свое лучшее начальное состояние в начальном состоянии. В этом примере задайте [100; 100; 0; 0].
Укажите своё доверие в том, что ваша оценка состояния догадывается в начальной ковариации. В этом примере задайте 10. Программа интерпретирует это значение, так как значения истинного состояния, вероятно, находятся в пределах вашей первоначальной оценки. Можно задать отдельное значение для каждого состояния путем установки Initial covariance
как вектор. Можно задать перекрестные корреляции в этой неопределенности, задав ее как матрицу.
Поскольку существует два датчика, нажмите Add Measurement, чтобы задать вторую функцию измерения.
Измерение 1
Укажите имя функции измерения, radarMeasurementFcn
, в Function.
Выберите Nonadditive
в раскрывающемся списке Measurement Noise, потому что вы явным образом заявили, как шум процесса влияет на измерения в вашей функции.
Задайте измерение шумовую ковариацию как [0,05 ^ 2 0; 0 0,05 ^ 2] по обсуждению в разделе Sensor Моделирования - Radar.
Измерение 2
Укажите имя функции измерения, gpsMeasurementFcn
, в Function.
Эта модель датчика имеет аддитивный шум. Поэтому задайте шум измерения GPS следующим Additive
в раскрывающемся списке Measurement Noise.
Задайте измерение шумовую ковариацию как [100 0; 0 100].
На вкладке Multirate, поскольку два датчика работают с различными скоростями дискретизации, выполните следующее строение:
Выберите Enable multirate operation
.
Задайте значение шага расчета переходов между состояниями. Время переходной выборки состояния должно быть наименьшим, а все шаги расчета измерения должны быть целым числом, кратным времени переходной выборки состояния. Задайте шаг расчета перехода состояния как 0.05, шаг расчета самого быстрого измерения. Хотя в этом примере и не требуется, возможно иметь меньший шаг расчета для перехода состояния, чем все измерения. Это означает, что будут некоторые шаги расчета без каких-либо измерений. Для этих шагов расчета фильтр генерирует предсказания состояния, используя функцию перехода состояния.
Задайте шаг расчета Measurement 1 (Radar) 0,05 секунд и Measurement 2 (GPS) 1 секунды.
Протестируйте эффективность расширенного фильтра Калмана путем симуляции сценария, где объект перемещается в квадратном шаблоне со следующими маневрами:
При t = 0 объект начинается с
Он направляется на север до t = 20 секунд.
Он направляется на восток между t = 20 и t = 45 секундами.
Он направляется на юг между t = 45 и t = 85 секундами.
Он направляется на запад между t = 85 и t = 185 секундами.
Сгенерируйте значения истинного состояния, соответствующие этому движению:
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 секунды.
Проверьте оценку состояния: Валидация неразумной и расширенной эффективности фильтра Калмана обычно выполняется с помощью обширных симуляций Монте-Карло. Для получения дополнительной информации см. "Валидация оценки состояния в режиме онлайн" в Simulink ".
Сгенерируйте код: Блоки Unscented и Extended Фильтра Калмана поддерживают генерацию C и Код С++ с помощью программного обеспечения Simulink Coder™. Функции, которые вы предоставляете этим блокам, должны соответствовать ограничениям программного обеспечения MATLAB Coder™ (если вы используете функции MATLAB для моделирования системы) и программного обеспечения Simulink Coder (если вы используете блоки Simulink Function для моделирования своей системы).
В этом примере показано, как использовать блок Расширенный фильтр Калмана в System Identification Toolbox. Вы оценивали положение и скорость объекта с двух разных датчиков, работающих с различными частотами дискретизации.
close_system('multirateEKFExample', 0); rmpath(fullfile(matlabroot,'examples','ident','main')) % remove example data
Extended Kalman Filter | Particle Filter | Unscented Kalman Filter