Кальман Филтеринг

Скрипт Open Live Script

Это тематическое исследование иллюстрирует проектирование и симуляцию Фильтра Калмана. И установившиеся и изменяющиеся во времени Фильтры Калмана рассматриваются.

Динамика объекта

Рассмотрите дискретный объект с аддитивным Гауссовым шумом $w [n]$ на входе $u [n]$ :

$\begin{array}{l} x [n + 1] = A x [n] + B (u [n] + w [n]) \\ y [n] = C x [n] . \end{array}$

Далее, позволить $y_{v} [n]$ будьте шумным измерением выхода $y [n]$ , с $v [n]$ обозначение шума измерения:

$y_{v} [n] = y [n] + v [n] .$

Следующие матрицы представляют динамику объекта.

A = [1.1269   -0.4940    0.1129;
     1.0000         0         0;
          0    1.0000         0];

B = [-0.3832;
      0.5919;
      0.5191];

C = [1 0 0];

Дискретный фильтр Калмана

Уравнения установившегося Фильтра Калмана для этой проблемы даны можно следующим образом.

Обновление измерения:

$x_{}^{ˆ} [n | n] = x_{}^{ˆ} [n | n - 1] + M (y_{v} [n] - C x_{}^{ˆ} [n | n - 1])$

Обновление времени:

$x_{}^{ˆ} [n + 1 | n] = A x_{}^{ˆ} [n | n] + B u [n]$

В этих уравнениях:

$x_{}^{ˆ} [n | n - 1]$ оценка $x [n]$ , учитывая прошлые измерения до $y_{v} [n - 1]$ .
$x_{}^{ˆ} [n | n]$ обновленная оценка на основе последнего измерения $y_{v} [n]$ .

Учитывая текущую оценку $x_{}^{ˆ} [n | n]$ , обновление времени предсказывает значение состояния на следующей выборке n + 1 (предиктор "один шаг вперед"). Обновление измерения затем настраивает это предсказание на основе нового измерения $y_{v} [n + 1]$ . Срок коррекции является функцией инноваций, то есть, несоответствия между измеренными и ожидаемыми значениями $y [n + 1]$ . Этим несоответствием дают:

$y_{v} [n + 1] - C x_{}^{ˆ} [n + 1 | n]$

Инновационное усиление M выбрано, чтобы минимизировать установившуюся ковариацию ошибки оценки, учитывая шумовые ковариации:

$E (w [n] w {[n]}^{T}) = Q E (v [n] v {[n]}^{T}) = R N = E (w [n] v {[n]}^{T}) = 0$

Можно объединить время и уравнения обновления измерения в одну модель в пространстве состояний, Фильтр Калмана:

$\begin{array}{l} x_{}^{ˆ} [n + 1 | n] = A (I - M C) x_{}^{ˆ} [n | n - 1] + [\begin{array}{cccccccccccccccccccc} B & A M \end{array}] [\begin{array}{cccccccccccccccccccc} u [n] \\ y_{v} [n] \end{array}] \\ y_{}^{ˆ} [n | n] = C (I - M C) x_{}^{ˆ} [n | n - 1] + C M y_{v} [n] . \end{array}$

Этот фильтр генерирует оптимальную оценку $y_{}^{ˆ} [n | n]$ из $y_{n}$ . Обратите внимание на то, что состояние фильтра $x_{}^{ˆ} [n | n - 1]$ .

Установившийся проект

Можно спроектировать установившийся Фильтр Калмана, описанный выше с функциональным kalman. Сначала задайте модель объекта управления с шумом процесса:

$\begin{array}{cccccccccccccccccccc} x [n + 1] = A x [n] + B u [n] + B w [n] \\ y [n] = C x [n] \end{array}$

Здесь, первое выражение является уравнением состояния, и вторым является уравнение измерения.

Следующая команда задает эту модель объекта управления. Шаг расчета установлен в-1, чтобы отметить модель как дискретная, не задавая шаг расчета.

Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');

При предположении, что Q = R = 1, спроектируйте дискретный Фильтр Калмана.

Q = 1; 
R = 1;
[kalmf,L,P,M] = kalman(Plant,Q,R);

Эта команда возвращает модель в пространстве состояний kalmf из фильтра, а также инноваций получают M.

Входные параметры kalmf вы и $y_{v}$ , и. Выходные параметры являются объектом выход и оценки состояния, $y_{e} = y_{}^{ˆ} [n | n]$ и $x_{}^{ˆ} [n | n]$ .

Поскольку вы интересуетесь выходной оценкой $y_{e}$ , выберите первый выход kalmf и отбросьте остальных.

kalmf = kalmf(1,:);

Чтобы видеть, как фильтр работает, сгенерируйте некоторые входные данные и случайный шум и сравните отфильтрованный ответ $y_{e}$ с истинным ответом y. Можно или сгенерировать каждый ответ отдельно или сгенерировать обоих вместе. Чтобы симулировать каждый ответ отдельно, используйте lsim с одним только объектом первый, и затем с объектом и фильтром, присоединенным вместе. Объединенная альтернатива симуляции детализирована затем.

Блок-схема ниже показов, как сгенерировать и истинные и отфильтрованные выходные параметры.

Можно создать модель в пространстве состояний этой блок-схемы с функциями parallel и feedback. Сначала создайте модель комплектного оборудования с u, w, v как входные параметры и y и $y_{v}$ (измерения) как выходные параметры.

a = A;
b = [B B 0*B];
c = [C;C];
d = [0 0 0;0 0 1];
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});

Затем используйте parallel сформировать параллельную связь следующего рисунка.

sys = parallel(P,kalmf,1,1,[],[]);

Наконец, замкните круг датчика путем соединения объекта выход $y_{v}$ отфильтровать вход $y_{v}$ с положительной обратной связью.

SimModel = feedback(sys,1,4,2,1);   % Close loop around input #4 and output #2
SimModel = SimModel([1 3],[1 2 3]); % Delete yv from I/O list

Получившаяся имитационная модель имеет w, v, u как входные параметры и y и $y_{e}$ как выходные параметры. Просмотрите InputName и OutputName свойства проверить.

SimModel.InputName

ans = 3x1 cell
    {'w'}
    {'v'}
    {'u'}

SimModel.OutputName

ans = 2x1 cell
    {'y'  }
    {'y_e'}

Вы теперь готовы симулировать поведение фильтра. Сгенерируйте синусоидальный вход u и векторы шума процесса и измерения w и против.

t = [0:100]';
u = sin(t/5);

n = length(t);
rng default
w = sqrt(Q)*randn(n,1);
v = sqrt(R)*randn(n,1);

Симулируйте ответы.

[out,x] = lsim(SimModel,[w,v,u]);

y = out(:,1);   % true response
ye = out(:,2);  % filtered response
yv = y + v;     % measured response

Сравните истинные и отфильтрованные ответы графически.

subplot(211), plot(t,y,'--',t,ye,'-'), 
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-'),
xlabel('No. of samples'), ylabel('Error')

Первый график показывает истинный ответ y (пунктирная линия) и отфильтрованный выход $y_{e}$ (сплошная линия). Второй график сравнивает погрешность измерения (штрихпунктир) с ошибкой оценки (тело). Этот график показывает, что уровень шума значительно уменьшался. Это подтверждено путем вычисления ошибок ковариации. Ошибочная ковариация прежде, чем отфильтровать (погрешность измерения):

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr)

MeasErrCov = 0.9992

Ошибочная ковариация после фильтрации (ошибка оценки) уменьшается:

EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)

EstErrCov = 0.4944

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

Изменяющийся во времени Фильтр Калмана является обобщением установившегося фильтра для изменяющихся во времени систем или систем LTI с неустановившейся шумовой ковариацией.

Рассмотрите следующее состояние объекта и уравнения измерения.

$\begin{array}{rcl} x [n + 1] & = & A x [n] + B u [n] + G w [n] \\ y_{v} [n] & = & C x [n] + v [n] . \end{array}$

Изменяющийся во времени Фильтр Калмана дан следующими рекурсиями:

Обновление измерения:

$\begin{array}{rcl} x_{}^{ˆ} [n | n] & = & x_{}^{ˆ} [n | n - 1] + M [n] (y_{v} [n] - C x_{}^{ˆ} [n | n - 1]) \\ M [n] & = & P [n | n - 1] C^{T} (R [n] + C P [n | n - 1] C^{T})^{- 1} \\ P [n | n] & = & (I - M [n] C) P [n | n - 1] . \end{array}$

Обновление времени:

$\begin{array}{rcl} x_{}^{ˆ} [n + 1 | n] & = & A x_{}^{ˆ} [n | n] + B u [n] \\ P [n + 1 | n] & = & A P [n | n] A^{T} + G Q [n] G^{T} . \end{array}$

Здесь, $x_{}^{ˆ} [n | n - 1]$ и $x_{}^{ˆ} [n | n]$ аналогичны описанному ранее. Дополнительно:

$\begin{array}{l} Q [n] = E (w [n] w {[n]}^{T}) \\ R [n] = E (v [n] v {[n]}^{T}) \\ P [n | n] = E ({x [n] - x [n | n]} {x [n] - x [n | n]}^{T}) \\ P [n | n - 1] = E ({x [n] - x [n | n - 1]} {x [n] - x [n | n - 1]}^{T}) . \end{array}$

Для простоты были пропущены индексы, указывающие на временную зависимость матриц пространства состояний.

Учитывая начальные условия $x [1 | 0]$ и $P [1 | 0]$ , можно выполнить итерации этих уравнений, чтобы выполнить фильтрацию. Необходимо обновить обоих оценки состояния $x [n | .]$ и ошибочные ковариационные матрицы $P [n | .]$ в каждый раз выборка.

Изменяющийся во времени проект

Чтобы реализовать эти рекурсии фильтра, сначала сгенерируйте шумные выходные измерения. Используйте шум процесса w и шум измерения v сгенерированный ранее.

sys = ss(A,B,C,0,-1);
y = lsim(sys,u+w);      
yv = y + v;

Примите следующие начальные условия:

$\begin{array}{cccccccccccccccccccc} x [1 | 0] = 0, & P [1 | 0] \end{array} = B Q B^{T}$

Реализуйте изменяющийся во времени фильтр с for цикл.

P = B*Q*B';         % Initial error covariance
x = zeros(3,1);     % Initial condition on the state
ye = zeros(length(t),1);
ycov = zeros(length(t),1); 

for i = 1:length(t)
  % Measurement update
  Mn = P*C'/(C*P*C'+R);
  x = x + Mn*(yv(i)-C*x);   % x[n|n]
  P = (eye(3)-Mn*C)*P;      % P[n|n]

  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n]
end

Сравните истинный и оцененный выход графически.

subplot(211), plot(t,y,'--',t,ye,'-')
title('Time-varying Kalman filter response')
xlabel('No. of samples'), ylabel('Output')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-')
xlabel('No. of samples'), ylabel('Output')

Первый график показывает истинный ответ y (пунктирная линия) и отфильтрованный ответ $y_{e}$ (сплошная линия). Второй график сравнивает погрешность измерения (штрихпунктир) с ошибкой оценки (тело).

Изменяющийся во времени фильтр также оценивает ковариацию errcov из ошибки оценки $y - y_{e}$ на каждой выборке. Постройте его, чтобы видеть, достиг ли ваш фильтр устойчивого состояния (как вы ожидаете со стационарным входным шумом).

subplot(211)
plot(t,errcov), ylabel('Error covar')

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

Сравните с ошибочной ковариацией оценки, выведенной из экспериментальных данных:

EstErr = y - ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)

EstErrCov = 0.4934

Это значение меньше, чем теоретическое значение errcov и близко к значению получен для установившегося проекта.

Наконец, обратите внимание что окончательное значение $M [n]$ и установившееся значение M инновационной матрицы усиления совпадает.

Mn

Библиография

[1] Grimble, M.J., Устойчивое Управление производственным процессом: Оптимальный Подход Проекта для Полиномиальных Систем, Prentice Hall, 1994, p. 261 и стр 443-456.

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