dosim.m

这是使用matlab语言编写的卡尔曼滤波程序,很不错的程序!

function S = dosim(T, cda, cdm, cm, sr_scale, sq_scale, stq_C, stq_F, stq_S)
%
% Run sim of level sensors
%
% Input:
%	T: truth struct with fields...
% 		L: actual levels struct w/ fields
% 				L.C (constant)
% 				L.F (filling)
%				L.S (sloshing)
% 				L.FS (filling and sloshing)
% 				L.S0 (slosh around 0)
% 		m: measured values struct
% 				level/linear fields m.l.C, m.l.F, m.l.S, m.l.FS
% 				angle/nonlinear fields m.a.C, m.a.F, m.a.S, m.a.FS
%		ts: time steps [second]
%		...: a bunch of other parameters that need to come from "truth"
%	cda: char specifying ACTUAL dynamics ('C','F','S','FS')
%	cdm: char specifying MODELED dynamics ('C','F','S','FS')
%	cm: char specifying the measurement type ('L', 'A')
%	sr_scale: meas noise factor (to play) -- {0.01, 0.1, 1, 10, 100}
%	sq_scale: process noise factor (to play) -- {0.01, 0.1, 1, 10, 100}
%

% misc globals
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]



%
% Set up filter parameters
%
% Four possibilities
% 1. Constant level (C)
% 2. Filling (F)
% 3. Sloshing (S)
% 4. Filling + Sloshing (FS)
%

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
	error('Incorrect number of input arguments for dosim.m.')
end

% Measurement model
if (cm == 'L')
	mtype = 1;
elseif (cm == 'A')
	mtype = 2;
else
	error('Unkown measurement model.');
end

% Set up the dynamic model parameters based on the input selector cdm
switch cdm
	case 'C' % Constant level
		% State dimension
		sd = 1;

		% Dynamic/process model
		% see Section 1.2.1 in document "models"
		A = zeros(1,1,T.nmeas);
		A(1,1,:) = 1;
		qc = (sq_scale*sq_C)^2; % scale qc then square for variance
		Q = zeros(1,1,T.nmeas);
		Q(1,1,:) = qc*dt;
		
		% Set aside space for results
		S.Xp = zeros(sd,T.nmeas); % predicted state (X)
		S.Xc = zeros(sd,T.nmeas); % corrected
		S.Pp = zeros(sd,sd,T.nmeas); % predicted covariance (P)
		S.Pc = zeros(sd,sd,T.nmeas); % corrected
		S.Zp = zeros(1,T.nmeas); % predicted measurement (Z)
		S.K  = zeros(sd,T.nmeas); % Kalman gain

		% Initialize filter
		L_init = L_max/2.0; % initial guess
		S.Xp(1,1) = L_init;
		S.Xc(1,1) = L_init;
		S.Pp(1:1,1:1,1) = L_max^2;
		S.Pc(1:1,1:1,1) = L_max^2;
	
	case 'F' % Filling
		% State dimension
		sd = 2;

		% Dynamic/process model
		% see Section 1.2.2 and equations (4) and (5) in document "models"
		A = zeros(sd,sd,T.nmeas);
		for m=1:T.nmeas
			A(1,1,m) = 1;
			A(2,2,m) = 1;
			A(1,2,m) = dt;
		end
		Q = zeros(sd,sd,T.nmeas);
		qc = (sq_scale*sq_F)^2; % scale qc then square for variance
		for m=1:T.nmeas
			Q(1,1,m) = qc*dt^3/3.0;
			Q(1,2,m) = qc*dt^2/2.0;
			Q(2,1,m) = Q(1,2,m);
			Q(2,2,m) = qc*dt;
		end
		
		% Set aside space for results
		S.Xp = zeros(sd,T.nmeas); % predicted state (X)
		S.Xc = zeros(sd,T.nmeas); % corrected
		S.Pp = zeros(sd,sd,T.nmeas); % predicted covariance (P)
		S.Pc = zeros(sd,sd,T.nmeas); % corrected
		S.Zp = zeros(1,T.nmeas); % predicted measurement (Z)
		S.K  = zeros(sd,T.nmeas); % Kalman gain

		% Initialize filter
		L_init = L_max/2.0; % initial guess
		S.Xp(1,1) = L_init;
		S.Xc(1,1) = L_init;
		S.Pp(:,:,1) = [L_max^2,0;0,(L_max/stime)^2];
		S.Pc(:,:,1) = [L_max^2,0;0,(L_max/stime)^2];
			
	case 'S' % Sloshing
		% State dimension
		sd = 2;

		% Dynamic/process model
		% see Section 1.2.3 and equations (6) and (7) in document "models"
		A = zeros(sd,sd,T.nmeas);
		w  = 2*pi*sf; % omega
		for m=1:T.nmeas
			A(1,1,m) = 1;
			A(2,2,m) = 1;
			A(1,2,m) = w*cos(w*T.ts(m)+sp*d2r);
		end
		Q  = zeros(sd,sd,T.nmeas);
		qc = (sq_scale*sq_S)^2; % scale qc then square for variance
		for m=1:T.nmeas
			Q(1,1,m) = qc*w^2*cos(w*T.ts(m))^2*dt*(3*T.ts(m)^2+3*T.ts(m)*dt+dt^2)/3.0;
			Q(1,2,m) = w*cos(w*T.ts(m))*qc*dt*(2*T.ts(m) + dt)/2.0;
			Q(2,1,m) = Q(1,2,m);
			Q(2,2,m) = qc*dt;
		end
		
		% Set aside space for results
		S.Xp = zeros(sd,T.nmeas); % predicted state (X)
		S.Xc = zeros(sd,T.nmeas); % corrected
		S.Pp = zeros(sd,sd,T.nmeas); % predicted covariance (P)
		S.Pc = zeros(sd,sd,T.nmeas); % corrected
		S.Zp = zeros(1,T.nmeas); % predicted measurement (Z)
		S.K  = zeros(sd,T.nmeas); % Kalman gain

		% Initialize filter
		L_init = L_max/2.0; % initial guess
		S.Xp(1,1) = L_init;
		S.Xc(1,1) = L_init;
		S.Xp(2,1) = T.sm; % temp - cheat
		S.Xc(2,1) = T.sm;
		S.Pp(:,:,1) = [L_max^2,0;0,(L_max/2)^2];
		S.Pc(:,:,1) = [L_max^2,0;0,(L_max/2)^2];
			
	case 'FS' % Filling and Sloshing
		% State dimension
		sd = 3;

		% Dynamic/process model
		% see Section 1.2.4 and equations (8) and (9) in document "models"
		A = zeros(sd,sd,T.nmeas);
		w  = 2*pi*sf; % omega
		for m=1:T.nmeas
			A(1,1,m) = 1;
			A(2,2,m) = 1;
			A(3,3,m) = 1;
			A(1,2,m) = dt;
			A(1,2,m) = w*cos(w*T.ts(m)+sp*d2r);
		end
		Q  = zeros(sd,sd,T.nmeas);
		qcs = (sq_scale*sq_S)^2; % scale qc then square for variance
		qcf = (sq_scale*sq_F)^2; % scale qc then square for variance
		for m=1:T.nmeas
			Q(1,1,m) = dt*(dt^2+3*T.ts(m)^2+3*T.ts(m)*dt)*(qcf+w^2*cos(w*T.ts(m))^2*qcs)/3.0;
			Q(1,2,m) = qcf*dt*(2*T.ts(m) + dt)/2.0;
			Q(2,1,m) = Q(1,2,m);
			Q(1,3,m) = w*cos(w*T.ts(m))*qcs*dt*(2*T.ts(m)+dt)/2.0;
			Q(3,1,m) = Q(1,3,m);
			Q(2,2,m) = qcf*dt;
			Q(3,3,m) = qcs*dt;
		end
		
		% Set aside space for results
		S.Xp = zeros(sd,T.nmeas); % predicted state (X)
		S.Xc = zeros(sd,T.nmeas); % corrected
		S.Pp = zeros(sd,sd,T.nmeas); % predicted covariance (P)
		S.Pc = zeros(sd,sd,T.nmeas); % corrected
		S.Zp = zeros(1,T.nmeas); % predicted measurement (Z)
		S.K  = zeros(sd,T.nmeas); % Kalman gain

		% Initialize filter
		L_init = L_max/2.0; % initial guess
		S.Xp(1,1) = L_init;
		S.Xc(1,1) = L_init;
		S.Xp(3,1) = T.sm; % temp - cheat
		S.Xc(3,1) = T.sm;
		S.Pp(:,:,1) = [L_max^2,0,0; 0,(L_max/stime)^2,0; 0,0,(L_max/2)^2];
		S.Pc(:,:,1) = [L_max^2,0,0; 0,(L_max/stime)^2,0; 0,0,(L_max/2)^2];
			
	otherwise
		error('Unknown dynamic model type.');
end

% Choose the set of measurements based on the input selector cda (actual dynamics)
switch cda
	case 'C' % Constant level
		% Measurement model
		if (mtype == 1)
			Za = T.m.l.C; % actual measurements
		else
			Za = T.m.a.C; % actual measurements
		end
	case 'F' % Filling level
		% Measurement model
		if (mtype == 1)
			Za = T.m.l.F; % actual measurements
		else
			Za = T.m.a.F; % actual measurements
		end
	case 'S' % Sloshing level
		% Measurement model
		if (mtype == 1)
			Za = T.m.l.S; % actual measurements
		else
			Za = T.m.a.S; % actual measurements
		end
	case 'FS' % Filling and Sloshing level
		% Measurement model
		if (mtype == 1)
			Za = T.m.l.FS; % actual measurements
		else
			Za = T.m.a.FS; % actual measurements
		end
	otherwise
		error('Unknown dynamic model type.');
end

% Measurement noise model
if (mtype == 1)
	% see Section 2.1 in document "models"
	R = (srl_m * kl)^2;
else
	% see Section 2.2 in document "models"
	R = (sra_d * ka)^2;
end



% Loop through all measurements
for k = 2:T.nmeas

	% predict
	S.Xp(:,k)   = A(:,:,k)*S.Xc(:,k-1);
	S.Pp(:,:,k) = A(:,:,k)*S.Pc(:,:,k-1)*A(:,:,k)' + Q(:,:,k);

	% measurement prediction and Jacobian
	S.Zp(k) = meas(S.Xp(:,k),mtype,sd);
	H       = measJacobian(S.Xp(:,k),mtype,sd);

	% correct
	S.K(:,k)    = S.Pp(:,:,k)*H'*inv(H*S.Pp(:,:,k)*H' + R); % Kalman gain
	S.Xc(:,k)   = S.Xp(:,k) + S.K(:,k)*(Za(k)-S.Zp(k));
	S.Pc(:,:,k) = (eye(sd) - S.K(:,k)*H)*S.Pp(:,:,k);

end

return



% local subroutine for measurement model
function Zp = meas(Xp,mtype,sd)
	global db df ka r2d;
	
	if (mtype == 1)
		% Linear measurement model
		% see Section 2.1 in document "models"
		H = measJacobian(Xp,mtype,sd);
		Zp = H*Xp;
	else 
		% Angular measurement model
		% see Section 2.2 and equation (13) in document "models"
		as = (db-Xp(1))/df;
		if (as > 1)
			as = 1;
		elseif (as < -1)
			as = -1;
		end
		Zp = r2d*ka*asin(as);
	end
return

% local subroutine for measurement model Jacobian
function H = measJacobian(Xp,mtype,sd)
	global db df ka kl r2d;
	
	
	if (mtype == 1)
		% Linear measurement model
		% see Section 2.1 in document "models"
		H = zeros(1,sd);
		H(1,1) = kl;
	else
		% Angular measurement model
		% see Section 2.2 and equation (14) in document "models"
		H = zeros(1,sd);
		as2 = ((db-Xp(1))/df)^2;
		H(1,1) = -r2d*ka/(df*sqrt(abs(1-as2)));
	end
return