www.gusucode.com > 惯性导航系统仿真程序 SIMULINK环境-SIMULINK源码程序 > code/KF10.m

    %%FILENAME:  KF5.m
%%LIANZI       2006-03-19  
%%FUNCTION:  The Kalman Filter function
%%

function  [X,P,K]=KF10(A,B,C,Z,X0,P0,Q0,R0,dt)

[phi , gama]=c2d(A, B, dt);

%%%1
%%%考虑计算可观测性判别矩阵的秩rank(M),M = [C' phi'*C' phi'^2*C' ... phi'^(n-1)*C']
%%%由此判断可观的状态数
%%M1 = phi'* C';
%M2 = phi'* M1;
%M3 = phi'* M2;
%M4 = phi'* M3;
%M = [ C' M1  M2  M3  M4];
Rk = 1;
%%%2
%%%离散系统的噪声方差阵有必要按照秦永元的p45 (2.2.31)式计算吗？看一下英文书的例子
%%%拙见： 函数c2d已经完成了这一步工作吧。是否有必要自己来计算离散化呢？
%%%试一下c2d的各种不同近似方法对应的参数，零阶保持不是最好的

X_1 = phi * X0;
P_1 = phi * P0 * phi' + gama * Q0 * gama' ;
K = P_1 * C' * inv( C * P_1 * C' + R0 );  
P = ( eye(10) - K * C ) * P_1 * ( eye(10) - K * C )' + K * R0 * K' ;
X = X_1 + K * ( Z - C * X_1);
% X_1 = phi * X0;
% P_1 = inv(inv(phi' * P0 * phi + gama* Q0 * gama')+C'*inv(R0)*C) ;
% K = P_1 * C' * inv( C * P_1 * C' + R0 );  
% P=P_1;
% % P = ( eye(10) - K * C ) * P_1 * ( eye(10) - K * C )' + K * R0 * K' ;
% X = X_1 + K * ( Z - C * X_1);