truth.m

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

function T = truth(seed);
%
% Generate truth signals
%
% Input:
% 	seed: random number seed (allows it to generate repeatable results)
%
% Output:
%	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]
%

% misc globals
d2r = pi/180.0; % constant to convert [degree] to [radian]
r2d = 180.0/pi; % constant to convert [radian] to [degree]

% seed random number generator
randn('state',seed);

% temporal parameters
T.stime = 5; % length of sim [second]
T.mrate = 50; % measurement rate [1/second]
T.nmeas = T.stime*T.mrate + 1;

% water level limits
T.L_min = 0.3; % where start filling, etc. [meter]
T.L_max = 1.0; % full [meter]

% Angular/non-linear float parameters
T.db = T.L_max + 0.02; % base for angular sensor, just above the max water [meter]
T.df = 1.25*T.db; % angular sensor arm w/ float, long enough to hit bottom on empty [meter]
T.ka = 1.0;

% Level/linear float parameters
T.kl = 1.0;

% measurement noise magnitudes
T.srl_m = 0.01*T.L_max; % stdev of level/linear sensor noise [meter]
T.sra_d = 0.01*90; % stdev of angule/non-linear sensor noise [degree]

% Filling parameters
T.delay = 1; % delay to start of filling [second]
frate = (T.L_max - T.L_min) / (T.stime - T.delay + 1/T.mrate); % fill rate [meter/second]
fmeas = frate/T.mrate; % amount filled per measurement [meter/meas]
mf = T.delay*T.mrate; % meas of first fill motion

% Sloshing (sinusoidal) parameters
T.sf = 10/T.stime; % slosh/sin frequency [1/second]
T.sp = 0; % slosh phase [degree]
T.sm = 0.05; % slosh magnitude [meter]

%
% Generate signals
%
% Four possibilities
% 1. Constant level (C)
% 2. Filling (F)
% 3. Sloshing (S)
% 4. Filling + Sloshing (FS)
%

% sample times
T.ts = 0:1/T.mrate:T.stime;

% noise signals for each sensor type
n.l = T.srl_m*randn(1,T.nmeas); % level/linear noise
n.a = T.sra_d*randn(1,T.nmeas); % angle/non-linear noise

%
% actual/true levels (generate same number/resolution as measurements)
%

% 1. Constant level (C)
% see equation (1) in document "models"
L.C = repmat(T.L_min,1,T.nmeas); % constant signal
m.l.C = T.kl*L.C + n.l; % level measured = signal + level/linear noise
m.a.C = T.ka*r2d*asin((T.db-L.C)/T.df) + n.a; % angle measured = signal + angle/non-linear noise

% 2. Filling (F)
% see equation (2) in document "models"
L.F = zeros(1,T.nmeas);
L.F(1:mf-1) = T.L_min;
L.F(mf:T.nmeas) = T.L_min:fmeas:T.L_max;
m.l.F = T.kl*L.F + n.l; % level measured = signal + level/linear noise
m.a.F = T.ka*r2d*asin((T.db-L.F)/T.df) + n.a; % angle measured = signal + angle/non-linear noise

% 3. Sloshing (S)
% see equation (3) in document "models"
L.S0 = T.sm*sin(2*pi*T.ts*T.sf + T.sp*d2r); % around 0
L.S = T.L_min + L.S0;
m.l.S = T.kl*L.S + n.l; % level measured = signal + level/linear noise
m.a.S = T.ka*r2d*asin((T.db-L.S)/T.df) + n.a; % angle measured = signal + angle/non-linear noise

% 4. Filling + Sloshing (FS)
% see equations (2) and (3) in document "models"
L.FS = L.F + L.S0; % sum of filling and sloshing (slosh around 0 not min)
m.l.FS = T.kl*L.FS + n.l; % level measured = signal + level/linear noise
m.a.FS = T.ka*r2d*asin((T.db-L.FS)/T.df) + n.a; % angle measured = signal + angle/non-linear noise

% put true Level and measurement information in struct
T.L  = L;
T.m  = m;

return