chenmh_kalman.m

来自「Frequently used algorithms for numerical」· M 代码 · 共 28 行

function s = Chenmh_Kalman(x,A,H,Q,C,us,Cs);
[xd,T] = size(x);
sd = size(A,1);
ud = sd;
s = zeros(sd,T);
s0 = us;
M = Cs;
M1 = A*M*A'+Q;
K = M1*H'*(H*M1*H'+C)^-1;
s(:,1) = A*s0+K*(x(:,1)-H*A*s0);
M = (eye(sd)-K*H)*M1;
M_trace = trace(M);
for i = 2:T
    M1 = A*M*A'+Q;
    K = M1*H'*(H*M1*H'+C)^-1;
    s(:,i) = A*s(:,i-1)+K*(x(:,i)-H*A*s(:,i-1));
    M = (eye(sd)-K*H)*M1;
    M_trace= [M_trace; trace(M)];
end
% figure; plot(M_trace,'r+'); title('Trace of covariance')