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

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

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

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

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

где

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
Поддержка
Для просмотра документации необходимо авторизоваться на сайте