Оценка состояния с использованием изменяющегося во времени фильтра Калмана

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

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

Этот пример показывает, как оценить состояния линейных систем с помощью изменяющихся во времени фильтров Калмана в Simulink. Вы используете блок Фильтра Калмана из System Identification Toolbox/Estimators библиотека для оценки положения и скорости наземного транспортного средства на основе измерений шумного положения, таких как измерения датчика GPS. Модель объекта управления в фильтре Калмана имеет изменяющиеся во времени шумовые характеристики.

Введение

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

$x_e(t)$ и$x_n(t)$ являются ли положения транспортного средства на востоке и севере от источника,$\theta(t)$ ориентация транспортного средства на востоке и$u_\psi(t)$ угол поворота транспортного средства. -$t$ переменная непрерывного времени.

Модель Simulink состоит из двух основных частей: модели транспортного средства и фильтра Калмана. Это объясняется далее в следующих разделах.

open_system('ctrlKalmanNavigationExample');

Модель транспортного средства

Отслеживаемое транспортное средство представлено простой моделью точечной массы:

$$ \frac{d}{dt} \left[
\begin{array} {c}
 x_e(t) \\
 x_n(t) \\
 s(t) \\
 \theta(t)
\end{array} \right] = \left[
\begin{array} {c}
 s(t)\cos(\theta(t)) \\
 s(t)\sin(\theta(t)) \\
 (P \; \frac{u_T(t)}{s(t)} - A \; C_d \; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

где состояния транспортного средства:

$$ \begin{array} {ll}
x_e(t) \; & \textnormal{East position} \; [m] \\
x_n(t) \; & \textnormal{North position} \; [m] \\
s(t) \; & \textnormal{Speed} \; [m/s] \\
\theta(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

параметры транспортного средства:

$$ \begin{array} {ll}
P=100000 \; & \textnormal{Peak engine power} \; [W] \\
A=1 \; & \textnormal{Frontal area} \; [m^2] \\
C_d=0.3 \; & \textnormal{Drag coefficient} \; [Unitless] \\
m=1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

и входы управления:

$$ \begin{array} {ll}
u_T(t) \; & \textnormal{Throttle position in the range of -1 and 1} \; [Unitless] \\
u_\psi(t) \; & \textnormal{Steering angle} \; [deg] \\
\end{array} $$

Продольная динамика модели игнорирует сопротивление качению шин. Боковая динамика модели предполагает, что требуемый угол поворота руля может быть достигнут мгновенно, и игнорирует момент рыскания инерции.

Модель автомобиля реализована в ctrlKalmanNavigationExample/Vehicle Model подсистема. Модель Simulink содержит два ПИ-контроллеров для отслеживания желаемой ориентации и скорости для автомобиля в ctrlKalmanNavigationExample/Speed And Orientation Tracking подсистема. Это позволяет вам задать различные условия работы автомобиля и протестировать эффективность фильтра Калмана.

Проект фильтра Калмана

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

$$ \hat{x}[n] = \left[
 \begin{array}{c}
 \hat{x}_e[n] \\
 \hat{x}_n[n] \\
 \hat{\dot{x}}_e[n] \\
 \hat{\dot{x}}_n[n]
 \end{array}
 \right]
$$

где

$$ \begin{array} {ll}
\hat{x}_e[n] \; & \textnormal{East position estimate} \; [m] \\
\hat{x}_n[n] \; & \textnormal{North position estimate} \; [m] \\
\hat{\dot{x}}_e[n] \; & \textnormal{East velocity estimate} \; [m/s] \\
\hat{\dot{x}}_n[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

$\dot{x}$Термины обозначают скорости, а не производный оператор.$n$ - индекс в дискретном времени. Модель, используемая в фильтре Калмана, имеет вид:

$$ \begin{array} {rl}
\hat{x}[n+1] &= A\hat{x}[n] + Gw[n] \\
y[n] &= C\hat{x}[n] + v[n]
\end{array} $$

где$\hat{x}$ - вектор состояния$y$, - измерения, -$w$ шум процесса и -$v$ шум измерения. Фильтр Калмана принимает, что и$w$ являются$v$ независимыми от нуля случайными переменными с известными отклонениями, и. $E[ww^T]=Q$$E[vv^T]=R$Здесь $E[wv^T]=N$матрицы A, G и C:

$$ 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]
$$

$$ C = \left[
 \begin{array}{c c c c}
 1 & 0 & 0 & 0 \\
 0 & 1 & 0 & 0
 \end{array}
 \right]
$$

где $T_s=1\;[s]$

Третья строка A и G моделируют восточную скорость как случайную прогулку:. $\hat{\dot{x}}_e[n+1]=\hat{\dot{x}}_e[n]+w_1[n]$В действительности положение является переменной непрерывного времени и является интегралом скорости с течением времени. $\frac{d}{dt}\hat{x}_e=\hat{\dot{x}}_e$Первая строка A и G представляет дискретное приближение к этому кинематическому отношению:. $(\hat{x}_e[n+1]-\hat{x}_e[n])/Ts=(\hat{\dot{x}}_e[n+1]+\hat{\dot{x}}_e[n])/2$Вторая и четвертая строки A и G представляют одинаковое соотношение между скоростью и положением на севере.

Матрица C представляет, что доступны только измерения положения. Датчик положения, такой как GPS, обеспечивает эти измерения со скоростью дискретизации 1Hz. Отклонение шума измерения, $v$матрица R, задано как. $R=50$Поскольку R задан как скаляр, блок фильтра Калмана принимает, что матрица R диагональна, его диагонали 50 и имеет совместимые размерности с y. Если шум измерения Гауссов, R = 50 соответствует 68% измерений положения внутри$\pm\sqrt{50}\;m$ или фактического положения в восточном и северном направлениях. Однако это предположение не обязательно для фильтра Калмана.

Элементы$w$ захвата, насколько скорость транспортного средства может измениться в течение одного шага расчета Ts. Отклонения технологического шума w, Q- матрицы, выбирается изменяющимся во времени. Он захватывает интуицию, которую типичные значения$w[n]$ меньше, когда скорость большая. Например, переход от 0 до 10 м/с легче, чем от 10 до 20 м/с. Конкретно, вы используете предполагаемые скорости на север и восток и функцию насыщения для построения Q [n]:

$$f_{sat}(z)=min(max(z,25),625)$$

$$ Q[n] = \left[
 \begin{array}{ c c }
 \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_e^2)} & 0 \\
 0 & \displaystyle 1+\frac{250}{f_{sat}(\hat{\dot{x}}_n^2)}
 \end{array}
 \right]
$$

Диагонали Q моделируют отклонение w, обратно пропорциональную квадрату предполагаемых скоростей. Функция насыщения препятствует тому, чтобы Q становился слишком большим или маленьким. Коэффициент 250 получают из данных времени ускорения методом наименьших квадратов, подобранных к 0-5, 5-10, 10-15, 15-20, 20-25 м/с для типового транспортного средства. Обратите внимание, что диагональный выбор Q представляет наивное предположение, что изменения скорости в северном и восточном направлениях являются некоррелированными.

Входные параметры и Setup блока Калмана

Блок 'Фильтр Калмана' находится в System Identification Toolbox/Estimators библиотека в Simulink. Это также в Control System Toolbox библиотека. Сконфигурируйте параметры блоков для оценки состояния в дискретном времени. Задайте следующие параметры параметров фильтра:

  • Временной интервал: Дискретное время. Выберите эту опцию, чтобы оценить состояния в дискретном времени.

  • Установите флажок Использовать измерение тока y [n] для улучшения xhat [n]. Это реализует вариант «оценщика токадискретного времени Калмана. Эта опция улучшает точность оценки и более полезна для медленных шагов расчета. Однако это увеличивает вычислительные затраты. В сложение этот вариант фильтра Калмана имеет прямое сквозное соединение, которое приводит к алгебраическому циклу, если фильтр Калмана используется в цикле обратной связи, который не содержит никаких задержек (сам цикл обратной связи также имеет прямые сквозные соединения). Алгебраический цикл может дополнительно повлиять на скорость симуляции.

Щелкните вкладку Options, чтобы задать опции ввода и вывода блоков:

  • Снимите флажок Добавить входной порт u. В модели объекта управления нет известных входов.

  • Установите флажок Выхода ошибки расчета ковариации Z. Матрица Z предоставляет информацию о доверии фильтра к оценкам состояния.

Нажмите Параметры модели, чтобы задать модель объекта управления и шумовые характеристики:

  • Источник модели: Отдельные матрицы A, B, C, D.

  • A: A. Матрица A задана ранее в этом примере.

  • C: C. Матрица C задана ранее в этом примере.

  • Источник начальной оценки: диалоговое окно

  • Начальные состояния x [0]: 0. Это представляет начальное предположение 0 для оценки положения и скорости в t = 0s.

  • Ошибка расчета состояния P [0]: 10. Предположим, что ошибка между вашим начальным предположением x [0] и его фактическим значением является случайной переменной со стандартным отклонением$\sqrt{10}$.

  • Установите флажок Использовать матрицы G и H (по умолчанию G = I и H = 0), чтобы задать не используемую по умолчанию матрицу G.

  • G: G. Матрица G задана ранее в этом примере.

  • H: 0. Шум процесса не влияет на измерения y, входящие в блок фильтра Калмана.

  • Снимите флажок Инвариантный по времени Q. Q- матрицы изменяется во времени и поставляется через блок inport Q. Блок использует изменяющийся во времени фильтр Калмана из-за этой настройки. Можно выбрать эту опцию, чтобы использовать инвариантный по времени фильтр Калмана. Инвариантный по времени фильтр Калмана выполняет несколько хуже для этой задачи, но легче в разработке и имеет более низкие вычислительные затраты.

  • R: R. Это ковариация шума измерения. $v[n]$Матрица R задана ранее в этом примере.

  • Н: 0. Предположим, что нет корреляции между шумами процесса и измерения.

  • Шаг расчета (-1 для унаследованного): Ts, который задан ранее в этом примере.

Результаты

Проверьте эффективность фильтра Калмана путем моделирования сценария, где транспортное средство совершает следующие маневры:

  • При t = 0 транспортное средство находится на, $x_e(0)=0$$x_n(0)=0$и является стационарной.

  • Направляясь на восток, ускоряется до 25 м/с. Он замедляется до 5 м/с при t = 50-х годах.

  • В t = 100-х годах поворачивает к северу и разгоняется до 20 м/с.

  • При t = 200 с совершает ещё один поворот к западу. Ускоряется до 25 м/с.

  • При t = 260-х он замедляется до 15 м/с и совершает постоянную скорость поворота на 180 степени.

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

sim('ctrlKalmanNavigationExample');

figure;
% Plot results and connect data points with a solid line.
plot(x(:,1),x(:,2),'bx',...
    y(:,1),y(:,2),'gd',...
    xhat(:,1),xhat(:,2),'ro',...
    'LineStyle','-');
title('Position');
xlabel('East [m]');
ylabel('North [m]');
legend('Actual','Measured','Kalman filter estimate','Location','Best');
axis tight;

Ошибка между измеренным и фактическим положением, а также ошибка между оценкой фильтра Калмана и фактическим положением:

% East position measurement error [m]
n_xe = y(:,1)-x(:,1);
% North position measurement error [m]
n_xn = y(:,2)-x(:,2);
% Kalman filter east position error [m]
e_xe = xhat(:,1)-x(:,1);
% Kalman filter north position error [m]
e_xn = xhat(:,2)-x(:,2);

figure;
% East Position Errors
subplot(2,1,1);
plot(t,n_xe,'g',t,e_xe,'r');
ylabel('Position Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xe,1)/numel(n_xe)),sprintf('Kalman f.: %.3f',norm(e_xe,1)/numel(e_xe)));
axis tight;
% North Position Errors
subplot(2,1,2);
plot(t,y(:,2)-x(:,2),'g',t,xhat(:,2)-x(:,2),'r');
ylabel('Position Error - North [m]');
xlabel('Time [s]');
legend(sprintf('Meas: %.3f',norm(n_xn,1)/numel(n_xn)),sprintf('Kalman f: %.3f',norm(e_xn,1)/numel(e_xn)));
axis tight;

Легенды графиков показывают измерения положения и ошибки расчета ($||x_e-\hat{x}_e||_1$и), $||x_n-\hat{x}_n||_1$нормированные количеством точек данных. Оценки фильтра Калмана имеют примерно на 25% меньше ошибок, чем необработанные измерения.

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

e_ve = xhat(:,3)-x(:,3); % [m/s] Kalman filter east velocity error
e_vn = xhat(:,4)-x(:,4); % [m/s] Kalman filter north velocity error
figure;
% Velocity in east direction and its estimate
subplot(2,1,1);
plot(t,x(:,3),'b',t,xhat(:,3),'r');
ylabel('Velocity - East [m]');
xlabel('Time [s]');
legend('Actual','Kalman filter','Location','Best');
axis tight;
subplot(2,1,2);
% Estimation error
plot(t,e_ve,'r');
ylabel('Velocity Error - East [m]');
xlabel('Time [s]');
legend(sprintf('Kalman filter: %.3f',norm(e_ve,1)/numel(e_ve)));
axis tight;

Легенда на графике ошибки показывает ошибку расчета скорости на восток$||\dot{x}_e-\hat{\dot{x}}_e||_1$, нормированную количеством точек данных.

Оценки скорости фильтра Калмана отслеживают фактические тренды скорости правильно. Уровни шума уменьшаются, когда транспортное средство двигается с высокими скоростями. Это в линию с дизайном Q- матрицы. Большие два шипа имеют значения t = 50s и t = 200s. Это времена, когда машина проходит через внезапное замедление и резкий поворот, соответственно. Изменения скорости в эти моменты намного больше, чем предсказания из фильтра Калмана, который основан на его Q- матрицей входе. Через несколько временных шагов оценки фильтра догоняют фактическую скорость.

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

Вы оценили положение и скорость транспортного средства, используя блок фильтра Калмана в Simulink. Динамика шума процесса модели изменялась во времени. Вы проверили эффективность фильтра путем симуляции различных маневров транспортного средства и случайным образом сгенерированного шума измерения. Фильтр Калмана улучшил измерения положения и предоставил оценки скорости для транспортного средства.

bdclose('ctrlKalmanNavigationExample');

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

Поддержка

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