Matlab: программа возвращает мусорные значения, помощь в правильном выполнении фильтра Калмана и оценке параметров

Я реализовал сглаживание Калмана с максимизацией ожидания на основе параметра Paper Оценка для линейной динамической системы. Все обозначения основаны на этой статье. Модель представляет собой БИХ-фильтр (AR(2))

y(t) =  0.195 *y(t-1) - 0.95*y(t-2) + w(t)

Представление пространства состояний:

x(t+1) = a^Tx(t) + w(t)

y(t) = C(t) + v(t)

Модель пространства состояний:

x(t+1) = Ax(t) + w(t)

y(t) = Cx(t) + v(t)

w(t) = N(0,Q) is the driving process noise 

v(t) = N(0,R)  is the measurement noise

Переписывая модель AR как представление пространства состояний:

SS_AR

Может кто-нибудь указать, где я сделал неправильно, чтобы код работал. Я следовал большей части последовательности и структуры из https://github.com/cswetenham/pmr/blob/master/toolboxes/lds/lds.m#L110

(1) Уравнению (26) требуется начальное значение для $x0$. Я передал x0 = mean(x,2) функции Predict. У меня есть сомнения в этом. Будет ли x0 и, следовательно, initx средним значением наблюдения y, которое дает скаляр, или это будут 2 значения (2 строки по 1 столбцу), поскольку пространство состояний равно AR (2). Я не уверен в этом.

(2) Если я возьму x0 = mean(x,2) и прокомментирую код после фильтрации Калмана, это даст правильные результаты для оценки состояния. Только от сглаживания оценка параметра неверна. Это неверно, потому что новое x0 = initx = x1sum/N становится скаляром, тогда как при инициализации это была матрица 2 на 1, где каждая строка является состоянием.

%%% Matlab script to simulate data and process usiung Kalman for the state
%%% estimation of AR(2) time series.:  y(t) =  0.195 *y(t-1) - 0.95*y(t-2) + excite_input(t);
% Linear system representation
% x_n+1 = A x_n + Bw_n
% y_n = Cx_n + v_n
% w = N(0,Q); v = N(0,R)
clc
clear all

T = 100;
order = 2;
  a1 = 0.195;
  a2 = -0.95;

A = [ a1  a2;
      1 0 ];
C = [ 1 0 ];
B = [1;
      0];


 x =[ rand(order,1) zeros(order,T-1)];% a sequence of 100 2-d vectors



sigma_2_w =1;
sigma_2_v = 0.01;



Q=eye(order);
P=Q;



%Simulate the steady state covariance matrix P
%P = A*P*A' + B*sqrt(sigma_2_w)*B';
 P = dlyap(A,B*B');

%Simulate AR model time series, x;


sqrtW=sqrtm(sigma_2_w);
excite_input=B*sqrtW*randn(1,T);
for t = 1:T-1
    x(:,t+1) = A*x(:,t) + excite_input(t+1);
end

%noisy observation

y = C*x + sqrt(sigma_2_v)*randn(1,T);



R  = sigma_2_v ;

z = y;
%X= x';
 x0=mean(x,2);
 YHAT = zeros(1,T);
 XHAT = zeros(2,T+1);

LL=[];
converged = 0;
previous_loglik = -Inf;
Y =y;
z = Y;


N = T;
max_iter = 500;
num_iter = 0;
initx = x0;
% V1 = var(initx);
loglik = 0;
V1 = P;
 while ~converged & (num_iter <= max_iter)
  initx = x0;

  k = length(initx);
  I=eye(k);
  xtt=zeros(2,T);   Vtt=zeros(2,2,T); xtt1=zeros(2,T); Vtt1=zeros(2,2,T); xhat_s = zeros(2,T);
  xtT=zeros(2,T); VtT=zeros(2,2,T); J=zeros(2,2,T); Vtt1T=zeros(2,2,T);  Ptsum = 0;
  x1sum = 0;
  P1sum = 0;
  A1=zeros(k);
  A2=zeros(k);
  XPred = zeros(2,T);
  Ptsum=zeros(k);
  xhat = zeros(2,1);
  Pcov = zeros(k,k);
  Kcur = 0;
  YX = 0;

%KAlman Filtering

for i =1:T

[xpred, Ppred] = predict(x0,V1, A, Q);
[nu, S] = innovation(xpred, Ppred, z(i), C, R);
[xnew, P, yhat, KalmanGain] = innovation_update_LDS(A, xpred, Ppred, V1, nu, S, C);
YHAT(i) = yhat;
Phat(i) = sqrt(C*P*C');
xtt(:,i) = xnew;  %xtt is the filtered state
Vtt(:,:,i) = P; %filtered covariance
Vtt1(:,:,i) = Ppred;
XPred(:,i) = A*xtt(:,i);


end 

KC = KalmanGain*C;

% 
% %Kalman Smoothing
% 
% 

KT = KalmanGain;

% %backward pass gets you E[x(t)|y(1:T)] from E[x(t)|y(1:t)]
t=T;
xtT(:,t) = xtt(:,t);
VtT(:,:,t) = Vtt(:,:,t);


% %SMOOTHING
 for t=(T-1):-1:1
     Vtt1(:,:,t) = A*Vtt(:,:,t)*A' + Q;
     J(:,:,t) = Vtt(:,:,t)*A'*inv(Vtt1(:,:,t+1)); %Eq(31)
     xtT(:,t) =  xtt(:,t) + ((xtT(:,t+1)- XPred(:,t))'*J(:,:,t))';  % Eq(32) xsmooth  modified the transpose
     VtT(:,:,t) = Vtt(:,:,t) + J(:,:,t)*(VtT(:,:,t+1)-Vtt1(:,:,t+1))*J(:,:,t)';  % Eq(33) Vsmooth or Psmooth
     Pt=VtT(:,:,t) + xtT(:,t)*xtT(:,t)'; 
     Ptsum=Ptsum+Pt;
     YX = YX+Y(:,t)'*xtT(:,t);  %For Eq(14)
      x1sum = x1sum + xtT(:,1);
    %  gama2 = gama2 + Pt - xtT(:,1)*xtT(:,1)' - VtT(:,:,1);

end
% Pt = VtT + xtT'*xtT;

% Pt = VtT(:,:,t) + xtT(:,t)'*xtT(:,t);  %P_t,t-1 = V_t,t-1^T + x_t^T * x_t^T'


 Sum_Pt_2T= Ptsum - Pt;  %A3  gama2
 A2= Ptsum + A2; %gama1

xhat_s = xtT; %smoothed estimate of x(t)


t= T;
 Pcov=(eye(2)-KC)*A*Vtt(:,:,t-1);
 A1=A1+Pcov+xtT(:,t)'*xtT(:,t-1);


for t= (T-1):-1:2
 Pcov =(Vtt(:,:,t) + J(:,:,t)*(Pcov - A*Vtt(:,:,t)))*J(:,:,t-1)';  %Eq(34)
 A1 = A1+Pcov+xtT(:,t)'*xtT(:,t-1);
 end; 

Rterm = (Y - C*xtt);
R_result = 0.5*Rterm' * inv(R)* Rterm;
R_sum_result = sum(sum(R_result));

Qterm = xtt(:,2:T)-(A*xtt(:,1:(T-1)));
Q_result = 0.5*Qterm' * inv(Q) * Qterm;
Q_sum_result = sum(sum(Q_result));

V1term = (xtt(:,1) -initx);
V1_result = 0.5 * V1term' * inv(V1) * V1term;

loglik_t = - R_sum_result - 0.5*T*log(det(R)) - Q_sum_result - 0.5*(T-1)*log(det(Q)) -  V1_result - 0.5*log(det(V1)) - 0.5*T*log(2*pi);



%STEP 2 Re-estimate B,Q,R,initx,initV1 via ML given x(t) estimate
 C=YX'*inv(Ptsum)/N;
 A=A1*inv(A2); 
 R1term = sum(Y.*Y)'/(T);
 R2term = diag(C*YX)/T;
 R = R1term - R2term;  % R = (1/T)*sum(Y.*Y - C.*xhat_s.*Y');
 Q=(1/(T-1))*diag(diag((Sum_Pt_2T-A*(A1')))); 
initx = x1sum/N;
x0 = initx;
V1 = Pt(:,:,1) - initx*initx';
  LL=[LL loglik_t];
  num_iter = num_iter+1
converged = em_converged(loglik, previous_loglik); %subfunction below
previous_loglik = loglik_t;


 end %while not converged
A

C
Q
R

function [xpred, Ppred] = predict(x0,P, A, Q)
xpred = A*x0;
Ppred = A*P*A' + Q;
end

function [nu, S] = innovation(xpred, Ppred, y, C, R)
nu = y - C*xpred; %% innovation

S = R + C*Ppred*C'; %% innovation covariance

end

function [xnew, Pnew, yhat, K] = innovation_update_LDS(A,xpred, Ppred,V1, nu, S, C)
% invP=inv(S);
% K = Ppred*C'*invP; %% Kalman gain
% xnew = xpred + K*nu; %% new state
% Pnew = Ppred - Ppred*K*C; %% new covariance
% yhat = C*xnew; % Observed value at time step i, assuming inferred state xnew
% xhat = A*xnew + K*nu;

K = Ppred*C'*inv(S); %% Kalman gain 2 rows 1 col (scalar
xnew = xpred + K*nu; %% new state
Pnew = Ppred - Ppred*K*C; %% new covariance
 yhat = C*xnew;
VVnew = (eye(2) - K*C)*A*V1;

end

function converged = em_converged(loglik, previous_loglik, threshold)
% EM_CONVERGED Has EM converged?
% [converged, decrease] = em_converged(loglik, previous_loglik, threshold)
%
% We have converged if
% |f(t) - f(t-1)| / avg < threshold,
% where avg = (|f(t)| + |f(t-1)|)/2 and f is log lik.
% threshold defaults to 1e-4.
% This stopping criterion is from Numerical Recipes in C p42
if nargin < 3
threshold = 1e-4;
end
%log likelihood should increase
if loglik - previous_loglik < -1e-3 % allow for a little imprecision
fprintf(1, '******likelihood decreased from %6.4f to %6.4f!\n', previous_loglik,loglik);
end
delta_loglik = abs(loglik - previous_loglik);
avg_loglik = (abs(loglik) + abs(previous_loglik) + eps)/2;
if (delta_loglik / avg_loglik) < threshold
converged = 1
else converged = 0
end

matlab implementation kalman-filter
person SKM    schedule 19.03.2015    source источник
comment
Есть ли причина не просто использовать, например. code.google.com/p/bnt/source/ просмотреть/trunk/Kalman/learn_kalman.m?   -  person Partha Lal    schedule 20.03.2015
comment
Мне нужно изменить много кода, чтобы он соответствовал моей модели. Лучший способ понять сложности и то, что происходит, — реализовать это самому. Более того, мои модели не ограничиваются регрессией и дополненной реальностью. Я нашел реализацию Кевина Мерфи очень жесткой. Кроме того, расчет логарифмической вероятности очень странный; он не следует какой-либо формуле. Отсюда и моя утомительная практика реализации всего этого.   -  person SKM    schedule 20.03.2015

Ответы (1)


arrow_upward
3
arrow_downward

Первое, с чего я всегда начинаю, когда смотрю на код Калмана, — это шаг обновления, в частности, обновление ковариации. В вашем коде это innovation_update_LDS

Стандартная форма, которую вы используете, это Pnew = Ppred - Ppred*K*C; %% new covariance, это неверно, она должна быть Pnew = Ppred - K*C*Ppred или чаще Pnew = (I - K*C)*Ppred;, где I=eye(len(K));

кроме того, я бы никогда не использовал эту форму уравнения. Используйте "форму Джозефа"

Pnew = (eye(2) - K*C) * Ppred * (eye(2)-K*C)' + K*R*K';

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

Три также, кажется, несколько ошибок в разделе фильтрации %KAlman. Я думаю, что это должно выглядеть больше так

%KAlman Filtering

for i =1:T
    if (i==1)
        [xpred, Ppred] = predict(x0,V1, A, Q);
    else
        [xpred, Ppred] = predict(xtt(:,i-1),Vtt(:,:,i-1), A, Q);
    end
    [nu, S] = innovation(xpred, Ppred, z(i), C, R);
    [xnew, Pnew, yhat, KalmanGain] = innovation_update_LDS(A, xpred, Ppred, V1, nu, S, C, R);
    YHAT(i) = yhat;
    Phat(i) = sqrt(C*Pnew*C');
    xtt(:,i) = xnew;  %xtt is the filtered state
    Vtt(:,:,i) = Pnew(:,:); %filtered covariance
end

ошибок могло быть и больше, но это все, что я успел найти. Удачи

person andrew    schedule 24.03.2015
comment
Спасибо за ваши усилия и время, чтобы решить проблему. Спасибо за знакомство с формой Джозефса. Одна из основных проблем с кодом, насколько я понимаю, заключается в шаге максимизации. переменная initx заменяется на x0. Инициализация x0 = среднее (x,2). Но x скрыт, так как же я могу взять его среднее значение? Другое дело — вероятность, и оценки не обновляются, поэтому все значения — NaN. Должен ли я обновить выражение логарифмического правдоподобия после этапа сглаживания и вставить сглаженные значения в логарифмическое правдоподобие или просто использовать отфильтрованные состояния в логарифмическом правдоподобии? - person SKM; 25.03.2015
comment
Будете ли вы просматривать весь код, чтобы исправить его, или я должен вернуться с любыми исправлениями, которые я могу сделать с моей стороны, до истечения срока действия награды, и тогда, возможно, вы сможете помочь мне с этого момента и далее? - person SKM; 25.03.2015
comment
ну, я думаю, вы можете использовать x0=mean(x,2) только потому, что в этой конкретной ситуации у вас есть все значения X. в реальном мире. вы бы использовали другое значение. Возможно, 0,0 приятно то, что, поскольку фильтр Калмана является интеллектуальным, поэтому при разумной инициализации он должен сходиться к фактическому состоянию всего через несколько итераций. Я не знаю, в чем здесь смысл EM (я не могу читать статью), поэтому я не знаю, где должен быть LLF, я предполагаю, что нужно использовать обновленные значения. Я вижу ваши значения изменений EM, используемые фильтром Калмана. Две вещи, редактирующие 1 набор значений, кажутся мне проблемой. - person andrew; 25.03.2015