卡爾曼濾波算法,matlab程序

ID:438396 · 發表于 2018-12-3 14:44

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
?

復制代碼

帳號		自動登錄	找回密碼
密碼			立即注冊

久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

卡爾曼濾波算法,matlab程序