kalman.m

卡尔曼滤波和自适应滤波 y

function [X,P,H]=kalman(Y,A,C,Q,R,x1,p1)
% Y: data observed (m*1)
% X: estimate of the state vector (n*1)
% A: state transition matrix (n*n)
% C: measure ment matrix (m*n)
% Q: correlation matrix of process noise vector w_k (n*n)
% R: correlation matrix of measurement noise vector v_k (m*m)
% P: correlation matrix of the error in X (n*n)
% H: kalman gain (n*m)
% Page 53
% by yyu 2006.3.19
format long;

n=length(x1)
% n is the dimension of state vector x
[m M]=size(Y)
% m is the dimension of output vector y
% M is the length of the observed data array

if n==1
    for k=2:M
    AA(:,:,k)=A(k);
    CC(:,:,k)=C(k);
    QQ(:,:,k)=Q(k);
    RR(:,:,k)=R(k);
    end
    A=AA;C=CC;Q=QQ;R=RR;
end

X(:,1)=x1;
P(:,:,1)=reshape(p1,n,n);
I=eye(length(p1));
for k=2:M
% while( P(k)-P(k-1)<norm(xx) )
    PP(:,:,k)=A(:,:,k)*P(:,:,k-1)*A(:,:,k)'+Q(:,:,k-1);
    H(:,:,k)=PP(:,:,k)*C(:,:,k)'*(C(:,:,k)*PP(:,:,k)*C(:,:,k)'+R(:,:,k))^-1;
    X(:,k)=A(:,:,k)*X(:,k-1)+H(:,:,k)*(Y(:,k)-C(:,:,k)*A(:,:,k)*X(:,k-1));
    P(:,:,k)=(I-H(:,:,k)*C(:,:,k))*PP(:,:,k);
end