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

Рассмотрим следующий дискретный объект с Гауссовым шумом w на входе и измерительным шумом v на выходе:

Цель состоит в том, чтобы спроектировать фильтр Калмана, чтобы оценить истинный выход объекта ${\mathit{y}}_{\mathit{t}}\left[\mathit{n}\right]=\mathit{y}\left[\mathit{n}\right]-\mathit{v}\left[\mathit{n}\right]$ на основе шумных измерений $\mathit{y}\left[\mathit{n}\right]$. Этот установившийся фильтр Калмана использует следующие уравнения для этой оценки.

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

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

Вот,

(Эти обновленные уравнения описывают current type estimator. Для получения информации о различии между current оценщики и delayed оценщики, см. kalman.)

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

Можно использовать kalman функция для разработки этого установившегося фильтра Калмана. Эта функция определяет оптимальное усиление М установившегося фильтра для конкретного объекта на основе ковариации Q технологического шума и ковариации R сенсорного шума, которую вы обеспечиваете. В данном примере используйте следующие значения для матриц пространства состояний объекта.

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;

В данном примере установите G = B, что означает, что технологический шум w является аддитивным входным шумом. Кроме того, установите H = 0, что означает, что входной шум w не оказывает прямого эффекта на выход y. Эти предположения дают более простую модель объекта управления:

Когда H = 0, можно показать, что ${\mathit{M}}_{\mathit{y}}=\mathit{C}{\mathit{M}}_{\mathit{x}}$ (см. kalman). Вместе эти допущения также упрощают уравнения обновления для фильтра Калмана.

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

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

Чтобы спроектировать этот фильтр, сначала создайте модель объекта управления с входом для w. Установите значение шага расчета -1 пометить объект как дискретный (без определенного шага расчета).

Ts = -1;
sys = ss(A,[B B],C,D,Ts,'InputName',{'u' 'w'},'OutputName','y');  % Plant dynamics and additive input noise w

Ковариация шума процесса Q и ковариация шума датчика R являются значениями, больше нуля, которые вы обычно получаете из исследований или измерений вашей системы. В данном примере задайте следующие значения.

Q = 2.3; 
R = 1; 

Используйте kalman команда для разработки фильтра.

[kalmf,L,~,Mx,Z] = kalman(sys,Q,R);

Эта команда проектирует фильтр Калмана, kalmf, модель пространства состояний, которая реализует уравнения обновления во времени и обновления измерения. Входами фильтра являются вход u объекта управления и выход y шумного объекта. Первый выход kalmf является оценкой $\hat{\mathit{y}}$ истинного выхода объекта, а остальные выходы являются оценками состояния $\hat{\mathit{x}}$.

В данном примере отбросьте оценки состояния и сохраните только первый выход, $\hat{\mathit{y}}$.

kalmf = kalmf(1,:);

Использование фильтра

Чтобы увидеть, как этот фильтр работает, сгенерируйте некоторые данные и сравните отфильтрованный ответ с истинной характеристикой объекта. Полная система показана на следующей схеме.

Чтобы симулировать эту систему, используйте sumblk чтобы создать вход для шума измерения v. Затем используйте connect чтобы присоединиться к sys и фильтр Калмана вместе такой, что u является общим входом и шумным выходом объекта управления y подает в другой вход фильтра. Результатом является симуляционная модель с входами w, v, и u и выходы yt (истинный ответ) и ye (отфильтрованный или предполагаемый ответ $\hat{\mathit{y}}$). Сигналы yt и ye являются выходами объекта управления и фильтра, соответственно.

sys.InputName = {'u','w'};
sys.OutputName = {'yt'};
vIn = sumblk('y=yt+v');

kalmf.InputName = {'u','y'};
kalmf.OutputName = 'ye';

SimModel = connect(sys,vIn,kalmf,{'u','w','v'},{'yt','ye'});

Чтобы симулировать поведение фильтра, сгенерируйте известный синусоидальный входной вектор.

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

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

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

Наконец, симулируйте ответ с помощью lsim.

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

lsim генерирует ответ на выходах yt и вы к входам, примененным в w, v, и u. Извлечение yt и ye каналы и вычислите измеренный отклик.

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

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

clf
subplot(211), plot(t,yt,'b',t,ye,'r--'), 
xlabel('Number of Samples'), ylabel('Output')
title('Kalman Filter Response')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

Как показывает второй график, фильтр Калмана уменьшает ошибку yt - y из-за шума измерения. Чтобы подтвердить это сокращение, вычислите ковариацию ошибки перед фильтрацией (ковариация ошибки измерения) и после фильтрации (ковариация ошибки расчета).

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

MeasErrCov = 0

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

EstErrCov = 0.3479

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

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

Изменяющийся во времени фильтр Калмана имеет следующие обновленные уравнения. В изменяющемся во времени фильтре обе ковариации ошибок $\mathit{P}\left[\mathit{n}\right]$ и инновационный прирост ${\mathit{M}}_{\mathit{x}}\left[\mathit{n}\right]$ может варьироваться со временем. Можно изменить уравнения обновления времени и измерения для расчета изменения времени следующим образом. (См. kalman для получения дополнительной информации об этих выражениях.)

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

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

Можно реализовать изменяющийся во времени фильтр Калмана в Simulink ® с помощью блока Kalman Filter. Для примера, демонстрирующего использование этого блока, см. Оценку состояния с использованием изменяющегося во времени фильтра Калмана. В данном примере реализуйте изменяющийся во времени фильтр в MATLAB ®.

Чтобы создать изменяющийся во времени фильтр Калмана, сначала сгенерируйте шумную реакцию объекта. Симулируйте реакцию объекта на входной сигнал u и технологический шум w заданный ранее. Затем добавьте шум измерения v к моделируемой функции true yt чтобы получить шумный ответ y. В этом примере ковариации векторов шума w и v не изменяйте со временем. Однако можно использовать ту же процедуру для нестационарного шума.

yt = lsim(sys,[u w]);   
y = yt + v;         

Затем реализуйте рекурсивные уравнения обновления фильтра в 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
  Mxn = P*C'/(C*P*C'+R);
  x = x + Mxn*(y(i)-C*x);   % x[n|n]
  P = (eye(3)-Mxn*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,yt,'b',t,ye,'r--')
xlabel('Number of Samples'), ylabel('Output')
title('Response with Time-Varying Kalman Filter')
legend('True','Filtered')
subplot(212), plot(t,yt-y,'g',t,yt-ye,'r--'),
xlabel('Number of Samples'), ylabel('Error')
legend('True - measured','True - filtered')

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

figure
plot(t,errcov) 
xlabel('Number of Samples'), ylabel('Error Covariance'),

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

Как и в установившемся случае, фильтр уменьшает ошибку из-за шума измерения. Чтобы подтвердить это сокращение, вычислите ковариацию ошибки перед фильтрацией (ковариация ошибки измерения) и после фильтрации (ковариация ошибки расчета).

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

MeasErrCov = 0.9871

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

EstErrCov = 0.3479

Наконец, когда изменяющийся во времени фильтр достигает устойчивого состояния, значения в матрице усиления Mxn совпадают с вычисленными kalman для установившегося фильтра.

Mx,Mxn

Mx = 3×1

    0.5345
    0.0101
   -0.4776

Mxn = 3×1

    0.5345
    0.0101
   -0.4776