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

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

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

В этом примере показано, как оценить состояния линейных систем с помощью изменяющихся во времени Фильтров Калмана в Simulink. Вы используете блок Kalman Filter из 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 содержит два ПИ-контроллера для отслеживания желаемой ориентации и скорости для автомобиля в 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, обеспечивает эти измерения на уровне частоты дискретизации 1 Гц. Отклонение шума измерения$v$, матрицы R, задано как$R=50$. Поскольку R задан как скаляр, блок Фильтра Калмана принимает, что матрица R является диагональной, ее диагонали равняются 50, и имеет совместимые размерности с y. Если шум измерения является Гауссовым, R=50 соответствует 68% измерений положения, являющихся в$\pm\sqrt{50}\;m$ или фактического положения в восточных и северных направлениях. Однако это предположение не необходимо для Фильтра Калмана.

Элементы$w$ получения, насколько скорость транспортного средства может переключить один шаг расчета Ts. Отклонение шума процесса w, матрицы Q, выбрано, чтобы быть изменяющимся во времени. Это получает интуицию, что типичные значения$w[n]$ меньше, когда скорость является большой. Например, движение от 0 до 10m/s легче, чем движение от 10 до 20m/s. Конкретно вы используете предполагаемые северные и восточные скорости и функцию насыщения, чтобы создать 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-25m/s ускоряющие данные времени для типового транспортного средства. Обратите внимание на то, что диагональ Q выбор представляет наивное предположение, что изменения скорости в северном и восточном направлении являются некоррелироваными.

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

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

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

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

Кликните по вкладке Options, чтобы установить импорт блока и опции выходного порта:

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

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

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

  • Источник модели: Индивидуум A, Б, C, D матрицы.

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

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

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

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

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

  • Выберите Use 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$и является стационарным.

  • Направляясь на восток, это ускоряется к 25m/s. Это замедляется к 5m/s в t=50s.

  • В t = 100 с, это поворачивается к северу и ускоряется к 20m/s.

  • В t = 200 с, это делает другой поворот к западу. Это ускоряется к 25m/s.

  • В t = 260 с, это замедляется к 15m/s и делает постоянную скорость 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/s]');
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/s]');
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

Поддержка

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