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

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

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

Описание проблемы

Учитывая следующий дискретный объект

где

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

D = 0;

разработайте Фильтр Калмана, чтобы оценить вывод y на основе шумных измерений yv [n] = C x [n] + v [n]

Установившийся проект фильтра Калмана

Можно использовать функциональный KALMAN, чтобы разработать установившийся Фильтр Калмана. Эта функция решает, что оптимальный установившийся фильтр получает M на основе ковариации шума процесса Q и ковариации шума датчика R.

Сначала задайте объект + шумовая модель. Внимание: установите шаг расчета на-1 отмечать объект как дискретный.

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

Задайте ковариацию шума процесса (Q):

Q = 2.3; % A number greater than zero

Задайте ковариацию шума датчика (R):

R = 1; % A number greater than zero

Теперь разработайте установившийся Фильтр Калмана с уравнениями

   Time update:         x[n+1|n] = Ax[n|n-1] + Bu[n] + Bw[n]
   Measurement update:  x[n|n] = x[n|n-1] + M (yv[n] - Cx[n|n-1])
                        where M = optimal innovation gain
using the KALMAN command:
[kalmf,L,~,M,Z] = kalman(Plant,Q,R);

Первый вывод Фильтра Калмана, KALMF является объектом выходная оценка y_e = Cx[n|n] и остающиеся выходные параметры, является оценками состояния. Сохраните только первый вывод y_e:

kalmf = kalmf(1,:);

M,   % innovation gain

M =

    0.5345
    0.0101
   -0.4776

Чтобы видеть, как этот фильтр работает, сгенерируйте некоторые данные и сравните отфильтрованный ответ с истинным ответом объекта:

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

% First, build a complete plant model with u,w,v as inputs and
% y and yv as outputs:
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'});

Затем, соедините модель объекта управления и Фильтр Калмана параллельно путем определения u как разделяемый вход:

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

Наконец, соединитесь, объект вывел yv к входному yv фильтра. Примечание: yv является 4-м входом SYS и также его 2-м выводом:

SimModel = feedback(sys,1,4,2,1);
SimModel = SimModel([1 3],[1 2 3]);     % Delete yv form I/O

Получившаяся имитационная модель имеет w, v, u как входные параметры и y, y_e как выходные параметры:

SimModel.inputname

ans =

  3x1 cell array

    {'w'}
    {'v'}
    {'u'}


SimModel.outputname

ans =

  2x1 cell array

    {'y'  }
    {'y_e'}

Вы теперь готовы моделировать поведение фильтра. Сгенерируйте синусоидальный (известный) входной вектор:

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

Сгенерируйте шум процесса и векторы шума датчика:

rng(10,'twister');
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);

Теперь моделируйте ответ с помощью LSIM:

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

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

Сравните истинный ответ с отфильтрованным ответом:

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

Как показано во втором графике, Фильтр Калмана уменьшает ошибку y-yv из-за шума измерения. Чтобы подтвердить это, сравните ошибочные ковариации:

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

Ковариация ошибки прежде, чем отфильтровать (погрешность измерения):

MeasErrCov

MeasErrCov =

    0.9871

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

EstErrCov

EstErrCov =

    0.3479

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

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

Время, отличаясь Фильтр Калмана имеет следующие уравнения обновления.

   Time update:        x[n+1|n] = Ax[n|n] + Bu[n] + Bw[n]
                       P[n+1|n] = AP[n|n]A' + B*Q*B'
   Measurement update:
                       x[n|n] = x[n|n-1] + M[n](yv[n] - Cx[n|n-1])
                                                         -1
                       M[n] = P[n|n-1] C' (CP[n|n-1]C'+R)
                       P[n|n] = (I-M[n]C) P[n|n-1]

Во-первых, сгенерируйте шумный ответ объекта:

sys = ss(A,B,C,D,-1);
y = lsim(sys,u+w);   % w = process noise
yv = y + v;          % v = meas. noise

Затем, реализуйте рекурсии фильтра в Цикле 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);
errcov = 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,'b',t,ye,'r--'),
xlabel('No. of samples'), ylabel('Output')
title('Response with time-varying Kalman filter')
subplot(212), plot(t,y-yv,'g',t,y-ye,'r--'),
xlabel('No. of samples'), ylabel('Error')

Время, отличаясь фильтр также оценивает выходную ковариацию во время оценки. Постройте выходную ковариацию, чтобы видеть, достиг ли фильтр устойчивого состояния (как мы ожидали бы со стационарным входным шумом):

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

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

Сравните ошибки ковариации:

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

Ковариация ошибки прежде, чем отфильтровать (погрешность измерения):

MeasErrCov

MeasErrCov =

    0.9871

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

EstErrCov

EstErrCov =

    0.3479

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

M,Mn

M =

    0.5345
    0.0101
   -0.4776


Mn =

    0.5345
    0.0101
   -0.4776

Внешние веб-сайты

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