卡尔曼滤波算法matlab,卡尔曼滤波算法,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

?unction 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

?