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

Рассмотрите следующий дискретный объект с Гауссовым шумом 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 введите средство оценки. Для получения информации о различии между current средства оценки и delayed средства оценки, смотрите kalman.)

Спроектируйте фильтр

Можно использовать kalman функционируйте, чтобы спроектировать этот установившийся Фильтр Калмана. Эта функция решает, что оптимальный установившийся фильтр получает M для конкретного объекта на основе ковариации шума процесса 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 подача в другой фильтр вводится. Результатом является имитационная модель с входными параметрами wV, и 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 и Вы к входным параметрам, примененным в wV, и u. Извлеките yt и Вы образовываете канал, и вычислите измеренный отклик.

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 к симулированному истинному ответу 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