kalman滤波matlab实现.doc

function S = dosim(T, cda, cdm, cm, sr_scale, sq_scale, stq_C, stq_F, stq_S) global d2r r2d; d2r = pi/180.0; % constant to convert [degree] to [radian] r2d = 180.0/pi; % constant to convert [radian] to [degree] % temporal parameters stime = T.stime; % length of sim [second] mrate = T.mrate; % measurement rate [1/second] dt = 1/mrate; % water level limits L_min = T.L_min; % where start filling, etc. [meter] L_max = T.L_max; % full [meter] % Measurement device parameters global db df ka kl; db = T.db; % Base for angular sensor, just above the max water [meter] df = T.df; % Angular sensor arm w/ float, long enough to hit bottom on empty [meter] ka = T.ka; % Angular/non-linear float scale constant kl = T.kl; % Level/linear float parameters scale constant % Sloshing (sinusoidal) parameters sf = T.sf; % slosh/sin frequency [1/second] sp = T.sp; % slosh phase [degree] % measurement noise magnitudes srl_m = sr_scale*T.srl_m; % stdev of level/linear sensor noise [meter] sra_d = sr_scale*T.sra_d; % stdev of angule/non-linear sensor noise [degree] if (nargin == 6) % normal situation stq_C(1) = 5.9609e-05; % Tuned Constant w/ linear meas stq_C(2) = 3.5936e-05; % Tuned Constant w/ angular meas sq_C = mean(stq_C); % avg of the two stq_F(1) = 1.9638; % Tuned Filling w/ linear meas stq_F(2) = 2.027; % Tuned Filling w/ angular meas sq_F = mean(stq_F); % avg of the two stq_S(1) = 5.9609e-05; % Tuned Sloshing w/ linear meas stq_S(2) = 5.9609e-05; % Tuned Sloshing w/ angular meas sq_S = mean(stq_S); % avg of the two % special tune case for filling and sloshing if (cdm == FS) stq_F(1) = 0.0028009; % Tuned Filling w/ linear meas stq_F(2) = 0.0021271; % Tuned Filling w/ angular meas sq_F = mean(stq_F); % avg of the two stq_S(1) = 0.0014462; % Tuned Sloshing w/ linear meas stq_S(2) = 0.0010254; % Tuned Sloshing w/ angular meas sq_S = mean(stq_S); % avg of the two end elseif (nargin == 9) % tuning the filters sq_C = stq_C; sq_F = stq_F; sq_S = stq_S; else err