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

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

Открыть скрипт

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

Введение

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

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

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

  • Фильтр частиц: Реализует алгоритм фильтра частиц дискретного времени.

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

  1. Смоделируйте свой объект и поведение датчика с помощью функций Simulink или MATLAB.

  2. Сконфигурируйте параметры блока.

  3. Симулируйте фильтр и анализируйте результаты завоевать доверие в эффективности фильтра.

  4. Разверните фильтр на своем оборудовании. Можно сгенерировать код для этих фильтров с помощью программного обеспечения Simulink Coder™.

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

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

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

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

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

  • Шум аддитивного процесса: $x[k+1] = f(x[k], u[k]) + w[k]$

  • Неаддитивный шум процесса: $x[k+1] = f(x[k], w[k], u[k])$

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

Функцией f (...) может быть функция MATLAB, которые выполняют ограничения MATLAB Coder™ или Функциональный блок Simulink. После того, как вы создаете 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. Для этого

  • Добавьте Simulink Function блокируйтесь к своей модели от Simulink/User-Defined Functions библиотека

  • Нажмите на имя, показанное на Функциональном блоке Simulink. Отредактируйте имя функции, и добавьте или удалите аргументы ввода и вывода по мере необходимости. В этом примере именем для функции изменения состояния является stateTransitionFcn. Это имеет один выходной аргумент (xNext) и два входных параметра (x, w).

  • Хотя это не требуется в этом примере, можно использовать любые сигналы от остальной части модели Simulink в Функции Simulink. Для этого добавьте Inport блоки из Simulink/Sources библиотека. Обратите внимание на то, что они отличаются, чем ArgIn и ArgOut блоки, которые установлены через подпись вашей функции (xNext = stateTransitionFcn (x, w)).

  • В Функциональном блоке Simulink создайте свои блоки Simulink использования функции.

  • Установите размерности для аргументов x, w ввода и вывода и xNext во вкладке Signal Attributes ArgIn и ArgOut блоки. Тип данных и размерности порта должны быть сопоставимы с информацией, которую вы предоставляете в Extended Kalman Filter блок.

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

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

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

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

  • Аддитивный шум измерения: $y[k] = h(x[k], u[k]) + v[k]$

  • Неаддитивный шум измерения: $y[k] = h(x[k], v[k], u[k])$

Здесь 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.

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, чтобы выполнить оценку. Вы задаете имена функций изменения состояния и измерения, начальное состояние и утверждаете ошибочную ковариацию и характеристики шума процесса и измерения.

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

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

  1. Задайте функцию изменения состояния, stateTransitionFcn, в Функции. Поскольку у вас есть якобиан этой функции, выберите Jacobian и задайте Функцию Якоби, stateTransitionJacobianFcn.

  2. Выберите Nonadditive в Шуме Процесса выпадающий список, потому что вы явным образом утвердили, как шум процесса влияет на состояния в вашей функции.

  3. Задайте ковариацию шума процесса как [0.2 0; 0 0.2]. Как объяснено в разделе Plant Modeling этого примера, шумовые части процесса задают случайный обход скоростей в каждом направлении. Диагональные термины приблизительно получают, насколько скорости могут переключить один шаг расчета функции изменения состояния. Недиагональные термины обнуляются, который является наивным предположением, что скоростные изменения северных и восточных направлений являются некоррелироваными.

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

  1. Задайте свою лучшую оценку начального состояния в начальном состоянии. В этом примере задайте [100; 100; 0; 0].

  2. Задайте свое доверие к вашему оценочному предположению состояния в Начальной ковариации. В этом примере задайте 10. Программное обеспечение интерпретирует это значение, как истинные значения состояния, вероятно, будут в$\pm\sqrt{10}$ вашей первоначальной оценки. Можно задать отдельное значение для каждого состояния установкой Initial covariance как вектор. Можно задать взаимные корреляции в этой неопределенности путем определения его как матрицы.

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

Измерение 1

  1. Задайте имя своей функции измерения, radarMeasurementFcn, в Функции.

  2. Выберите Nonadditive в Шуме Измерения выпадающий список, потому что вы явным образом утвердили, как шум процесса влияет на измерения в вашей функции.

  3. Задайте ковариацию шума измерения как [0.05^2 0; 0 0.05^2] на обсуждение в разделе Sensor Modeling - Radar.

Измерение 2

  1. Задайте имя своей функции измерения, gpsMeasurementFcn, в Функции.

  2. Эта модель датчика имеет аддитивный шум. Поэтому задайте шум измерения GPS как Additive в Шуме Измерения выпадающий список.

  3. Задайте ковариацию шума измерения как [100 0; 0 100].

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

  1. Выберите Enable multirate operation.

  2. Задайте шаг расчета изменения состояния. Шаг расчета изменения состояния должен быть самым маленьким, и все шаги расчета измерения должны быть целочисленным кратным шаг расчета изменения состояния. Задайте шаг расчета Изменения состояния как 0,05, шаг расчета самого быстрого измерения. Хотя не требуемый в этом примере, возможно иметь меньший шаг расчета для изменения состояния, чем все измерения. Это означает, что будут некоторые шаги расчета без любых измерений. Для этих шагов расчета фильтр генерирует предсказания состояния с помощью функции изменения состояния.

  3. Задайте Измерение 1 шаг расчета (Радар) как 0,05 секунды и Измерение 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 секунды.

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

  1. Подтвердите оценку состояния: валидация недушистой и расширенной эффективности Фильтра Калмана обычно делается с помощью обширных симуляций Монте-Карло. Для получения дополнительной информации смотрите, Подтверждают Онлайновую Оценку состояния в Simulink.

  2. Сгенерируйте код: Недушистые и Расширенные блоки Фильтра Калмана поддерживают генерацию C и Кода С++ с помощью программного обеспечения Simulink Coder™. Функции, которые вы предоставляете этим блокам, должны выполнить ограничения программного обеспечения MATLAB Coder™ (если вы используете функции MATLAB, чтобы смоделировать вашу систему), и программное обеспечение Simulink Coder (если вы используете Функциональные блоки Simulink, чтобы смоделировать вашу систему).

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

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

close_system('multirateEKFExample', 0);

Смотрите также

| |

Похожие темы

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

Поддержка

Для просмотра документации необходимо авторизоваться на сайте