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

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

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

Рассмотрите дискретный объект с аддитивным Гауссовым шумом $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 array
    {'w'}
    {'v'}
    {'u'}

SimModel.OutputName

ans = 2x1 cell array
    {'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.

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