Exponenta Banner
exponenta event banner

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

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

Открыть сценарий

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

Введение

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

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

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

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

Входы и настройка фильтра Kalman

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

  • Временная область: Дискретное время. Эта опция используется для оценки состояний дискретного времени.

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

Перейдите на вкладку «Параметры», чтобы задать параметры ввода и вывода блока:

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

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

Щелкните Параметры модели (Model Parameters), чтобы указать модель установки и характеристики шума:

  • Источник модели: отдельные матрицы 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-матрица изменяется во времени и подается через блок ввода Q. Блок использует изменяющийся во времени фильтр Калмана из-за этой настройки. Эту опцию можно выбрать для использования инвариантного по времени фильтра Калмана. Инвариантный по времени фильтр Калмана работает несколько хуже для этой задачи, но легче в проектировании и имеет более низкую вычислительную стоимость.

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

  • N: 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');

Документация по инструментам идентификации системы

Поддержка