Оценка состояний нелинейной системы с несколькими, многосветными датчиками

Этот пример использует:

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

Введение

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

Расширенный фильтр Калмана: Реализует алгоритм расширенного фильтра Калмана первого порядка в дискретном времени.
Сигма-точечный фильтр Калмана: Реализует дискретный алгоритм сигма-точечного фильтра Калмана.
Фильтр частиц: Реализует алгоритм фильтра частиц в дискретном времени.

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

Моделируйте поведение своего объекта и датчика с помощью функций MATLAB или Simulink.
Сконфигурируйте параметры блока.
Симулируйте фильтр и анализируйте результаты, чтобы получить доверие в эффективности фильтра.
Разверните фильтр на своем оборудовании. Вы можете сгенерировать код для этих фильтров с помощью программного обеспечения Simulink Coder™.

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

Если вы заинтересованы в блоке Particle Filter, смотрите пример «Оценка параметра и состояния в Simulink с использованием блока фильтра частицы».

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

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

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

Аддитивный технологический шум:
Неаддитивный шум процесса:

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

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

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

$\hat{x}[k] = \left[
 \begin{array}{c}
 \hat{x}_e[k] \\
 \hat{x}_n[k] \\
 \hat{v}_e[k] \\
 \hat{v}_n[k]
 \end{array}
 \right]
 \;
\begin{array} {ll}
\textnormal{East position estimate} \; [m] \\
\textnormal{North position estimate} \; [m] \\
\textnormal{East velocity estimate} \; [m/s] \\
\textnormal{North velocity estimate} \; [m/s] \\
\end{array}$

Вот $k$ индекс в дискретном времени. Используемое уравнение перехода состояния имеет неаддитивную форму, $\hat{x}[k+1] = A\hat{x}[k] + Gw[k]$ где является $\hat{x}$ вектором состояния и является $w$ шумом процесса. Фильтр принимает, что является $w$ независимой от нуля случайной переменной с известным отклонением. $E[ww^T]$ Матрицы A и G:

$A = \left[
 \begin{array}{c c c c}
 1 & 0 & T_s & 0 \\
 0 & 1 & 0 & T_s \\
 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 1
 \end{array}
 \right]
 \;\;\;\;\;\;\;\; G = \left[
 \begin{array}{c c}
 T_s/2 & 0 \\
 0 & T_s/2 \\
 1 & 0 \\
 0 & 1
 \end{array}
 \right]
$

где $T_s$ - шаг расчета. Третья строка A и G моделируют восточную скорость как случайную прогулку:. $\hat{v}_e[k+1]=\hat{v}_e[k]+w_1[k]$ В действительности положение является переменной непрерывного времени и является интегралом скорости с течением времени. $\frac{d}{dt}\hat{x}_e=\hat{v}_e$ Первая строка A и G представляет дискретное приближение к этому кинематическому отношению:. $(\hat{x}_e[k+1]-\hat{x}_e[k])/T_s = (\hat{v}_e[k+1]+\hat{v}_e[k])/2$ Вторая и четвертая строки 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% шума. Это может быть смоделировано следующим уравнением измерения:

$y_{radar}[k] = \left[
 \begin{array}{c}
 \sqrt{x_n[k]^2+x_e[k]^2} \; (1+v_1[k]) \\
 atan2(x_n[k], x_e[k]) \; (1+v_2[k]) \\
 \end{array}
 \right]
$

Вот $v_1[k]$ и $v_2[k]$ являются терминами шума измерения, каждый с отклонением 0,05 ^ 2. То есть большинство измерений имеют ошибки менее 5%. Шум измерения является неаддитивным, потому что $v_1[k]$ и $v_2[k]$ не просто добавляются к измерениям, но вместо этого они зависят от состояний x. В этом примере уравнение радиолокационного измерения реализуется с помощью блока Simulink Function.

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

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

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

$y_{GPS}[k] = \left[
 \begin{array}{c}
 x_e[k] \\
 x_n[k]
 \end{array}
 \right] +
 \left[
 \begin{array}{c}
 v_1[k] \\
 v_2[k]
 \end{array}
 \right]
$

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

Создайте эту функцию и сохраните ее в файле с именем 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. Программа интерпретирует это значение, так как значения истинного состояния, вероятно, находятся в пределах $\pm\sqrt{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 объект начинается с $x_e(0)=100\; \textnormal{[m]}, x_n(0)=100 \;\textnormal{[m]}$
Он направляется на север $\dot{x}_n=50\;\textnormal{[m/s]}$ до t = 20 секунд.
Он направляется на восток между $\dot{x}_n=40\;\textnormal{[m/s]}$ t = 20 и t = 45 секундами.
Он направляется на юг между $\dot{x}_n=-25\;\textnormal{[m/s]}$ t = 45 и t = 85 секундами.
Он направляется на запад между $\dot{x}_e=-10\;\textnormal{[m/s]}$ 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);

См. также

Extended Kalman Filter | Particle Filter | Unscented Kalman Filter

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