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

В этом примере используются:

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

Введение

Панель инструментов содержит три блока Simulink для нелинейной оценки состояния:

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

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

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

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

Если вы заинтересованы в блоке фильтра частиц, см. пример «Оценка параметров и состояний в 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, соответствующей ограничениям Coder™ MATLAB, или блоком Simulink Function. После создания f (...) необходимо указать имя функции и указать, является ли шум процесса аддитивным или неаддитивным в блоке расширенного фильтра Калмана.

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

$\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 = stateTransityFcn (x, w)).
В блоке функции Simulink создайте свою функцию, используя блоки 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.

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

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

Конструкция фильтра

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

На вкладке «Системная модель» диалогового окна блока задайте следующие параметры:

Переход к состоянию

Укажите функцию перехода состояния, stateTransitionFcn, в функции. Поскольку у вас есть якобиан этой функции, выберите якобиан и укажите якобианскую функцию, stateTransitionJacobianFcn.
Выбрать Nonadditive в выпадающем списке Шум процесса (Process Noise), поскольку вы явно указали, как шум процесса влияет на состояния в вашей функции.
Укажите ковариацию технологического шума как [0.2 0; 0 0.2]. Как поясняется в разделе «Моделирование установки» этого примера, термины шума процесса определяют случайное изменение скоростей в каждом направлении. Диагональные члены приблизительно фиксируют, насколько скорости могут изменяться в течение одного времени выборки функции перехода состояния. Внедиагональные члены установлены в ноль, что является наивным предположением, что изменения скорости в северном и восточном направлениях не коррелируются.

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

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

Поскольку имеется два датчика, щелкните Добавить измерение (Add Measurement), чтобы указать вторую функцию измерения.

Измерение 1

Укажите имя измерительной функции, radarMeasurementFcn, в функции.
Выбрать Nonadditive в выпадающем списке «Шум измерения», поскольку вы явно указали, как шум процесса влияет на измерения в вашей функции.
Задайте ковариацию шума измерения как [0,05 ^ 2 0; 0 0,05 ^ 2] согласно обсуждению в разделе «Моделирование датчиков - радар».

Измерение 2

Укажите имя измерительной функции, gpsMeasurementFcn, в функции.
Эта модель датчика имеет аддитивный шум. Поэтому укажите шум измерения GPS как Additive в раскрывающемся списке Measurement Noise.
Задайте ковариацию шума измерения как [100 0; 0 100].

Так как два датчика работают с разной частотой дискретизации, на вкладке Multipate rate выполните следующую настройку:

Выбрать Enable multirate operation.
Укажите время выборки перехода состояния. Время выборки перехода состояния должно быть наименьшим, а время выборки всех измерений должно быть целым кратным времени выборки перехода состояния. Укажите для параметра State Transition sample time (Время выборки перехода состояния) значение 0,05, т.е. время выборки для наиболее быстрого измерения. Хотя это и не требуется в этом примере, возможно иметь меньшее время выборки для перехода состояния, чем все измерения. Это означает, что будет некоторое время выборки без каких-либо измерений. Для этих времен выборки фильтр генерирует прогнозы состояния с использованием функции перехода состояния.
Задайте для параметра Measurement 1 sample time (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 Kalman Filter поддерживают генерацию кода C и C++ с помощью программного обеспечения Simulink Coder™. Функции, предоставляемые этим блокам, должны соответствовать ограничениям программного обеспечения MATLAB Coder™ (если для моделирования системы используются функции MATLAB) и программного обеспечения Simulink Coder (если для моделирования системы используются функциональные блоки Simulink).

Резюме

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

close_system('multirateEKFExample', 0);

См. также

Расширенный фильтр Калмана | Фильтр частиц | Незараженный фильтр Калмана

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