kalman滤波matlab实现

1、kalman滤波mat lab实现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 degreeZ to radianZ r2d =180.0/pi; % constant to convert radianZ to degree!% temporal parametersstime = T stime; % length of sim secondmrate = T mrate; % m

2、easurement rate 1/secondJdt = 1/mrate;% water level limitsL_min = T L_min; % where start filling, etc meter L_max = T L_max;% full meter% Measurement device parametersglobal db df ka kl:db = T db; % Base for angular sensor, just above the max watermeterdf = T df; % Angular sensor arm w/ float, long

3、enough to hit bottomon empty meterka = T ka; % Angular/non-1inear float scale constant kl = T kl; %Level/linear float parameters scale constant% Sloshing (sinusoidal) parameterssf = T sf; % slosh/sin frequency 1/secondZsp = T sp; % slosh phase degree% measurement noise magnitudessrl_m = sr_scale*T s

4、rl_m; % stdev of level/linear sensor noise metersra_d = sr_scale*T sra_d: % stdev of angule/non-1inear sensor noise degreeif (nargin = 6) % normal situationstq_C(l) = 5 9609e05; % Tuned Constant w/ linear measstq_C(2) = 3 5936e05; % Tuned Constant w/ angular meassjC = mean(stq_C) ; % avg of the twos

5、tq_F(l) = 1 9638; % Tuned Filling w/ linear measstq_F(2) = 2 027; % Tuned Filling w/ angular meassq_F = mean(stq_F); % avg of the twostq_S(1) = 5. 9609e-05; % Tuned Sloshing w/ linear measstjS(2) = 5. 9609e-05; % Tuned Sloshing w/ angular meassq_S = mean(stq_S) ; % avg of the two% special tune case

6、for filling and sloshing辻(cdm = FS)stq_F(l)=0.0028009; % Tuned Filling w/ linear measstq_F(2)=0.0021271: % Tuned Filling w/ angular meassq_F = mean(stq_F); % avg of the twostq_S(l)=0.0014462; % Tuned Sloshing w/ linear measstq_S(2)=0.0010254; % Tuned Sloshing w/ angular meassq_S = mean(stq_S); % avg

7、 of the twoA = zeros (1, 1, T nmeas);endelseif (nargin = 9) % tuning the filterssq_C = stq_C;sq_F = stq_F;sq_S = stq_S;elseerrorIncorrect number of input arguments for dosim. m. ) end% Measurement modelif (cm = L)mtype = 1;elseif (cm = A)mtype = 2;elseerror (,Unkown measurement mode 1.);end% Set up

8、the dynamic model parameters based on the input selectorcdmswitch cdmcase C % Constant level% State dimensionsd = 1;A = zeros (1, 1, T nmeas);% Dynamic/process model% see Section 1. 2 1 in document modelsA = zeros (sd, sd, T nmeas);A(l, 1,：)二1；qc = (sqscale*sq_C)2; % scale qc then square for varianc

9、eQ = zeros (1, 1, T nmeas);Q(l, 1, ：) = qc*dt;% Set aside space for resultsS Xp = zeros(sd, T .nmeas); % predicted state (X)S Xc = zeros (sd, T nmeas) ; % correctedS Pp = zeros (sd, sd, T .nmeas); % predicted covariance (P)S Pc = zeros (sd, sd, T .nmeas); % correctedS Zp = zeros(1, T .nmeas); % pred

10、icted measurement (Z)S K二zeros (sd, T .nmeas); % Kalman gain% Initialize filterL_init = L_max/2 0; % initial guessS.Xp(l, 1) = L_init;S. Xc(l, 1) = L_init;S. Pp(l: 1, 1:1,1)二L_max2;S. Pc (1:1, 1:1, 1)二L_max2case F % Filling% State dimensionsd = 2;% Dynamic/process model% see Section 1 2 2 and equati

11、ons (4) and (5) in document modelsA = zeros (sd, sd, T nmeas);for m=l :T.nmeasA(l, l,m)二1；A (2, 2, m) = 1;A(l, 2, m) = dt;endQ = zeros (sd, sd, T nmeas);qc = (sq_scale*sq_F)2; % scale qc then square for variancefor m=l:T.nmeasQ(l, 1, m) = qc*dt3/3.0;Q(l, 2, m) = qc*dt*2/2. 0;Q(2, l,m) = Q(l,2, m);Q(

12、2, 2, m) = qc*dt;end% Set aside space for resultsS Xp = zeros (sd, T .nmeas); % predicted state (X)S Xc = zeros (sd, T .nmeas); % correctedS Pp = zeros (sd, sd, T .nmeas); % predicted covariance (P)S Pc = zeros (sd, sd, T .nmeas); % correctedS Zp = zeros(1, T .nmeas); % predicted measurement (Z)S K二

13、zeros (sd, T .nmeas); % Kalman gain% Initialize filterL_init = L_max/2 0; % initial guessS. Xp(l, 1) = L_init;S Xp = zeros (sd, T.nmeas) : % predicted state (X)S. Xc(l, 1) = L_init;S. Pp(:,1) = L_max*2, 0;0, (L_max/stime) 2;S. Pc(:, :, 1) = L_max2, 0;0, (L_max/stime) 2;case S % Sloshing% State dimen

14、sionsd = 2;% Dynamic/process model% see Section 1.2.3 and equations (6) and (7) in document modelsA = zeros (sd, sd, T nmeas);w = 2*pi*sf: % omegaforHFI:T nmeasA(l, l,m) = 1；A (2, 2, m) = 1；A(l, 2, m) = w*cos (w*T. ts (m) +sp*d2r);endQ = zeros (sd, sd, T nmeas);qc = (sq-scale*sq_S)*2; % scale qc the

15、n square for variancefor nPl:T.nmeasQ(l, l,m)=qc*w2*cos(w*T. ts(m) 2*dt*(3*T. ts(m) 2+3*T. ts(m)*dt+dt2)/3. 0;Q(l, 2, m) = w*cos (w*T. ts (m) *qc*dt* (2*T. ts (m) + dt)/2. 0;Q(2, l,m) = Q(l,2, m);Q(2, 2, m) = qc*dt;S Xp = zeros (sd, T.nmeas) : % predicted state (X)end% Set aside space for resultsS X

16、c = zeros(sd, T nmeas); % correctedS Pp = zeros (sd, sd, T nmeas) ; % predicted covariance (P)S Pc = zeros (sd, sd, T nmeas) ; % correctedS Zp = zeros(1, T nmeas); % predicted measurement (Z)S K二zeros (sd, T .nmeas); % Kalman gain% Initialize filterL_init = L_max/2 0; % initial guessS. Xp(l, 1) = L_

17、init;S. Xc(l, 1) = L_init;S Xp(2, 1) = T sm; % temp一cheatS Xc 1) = T. sm;S. Pp(:,1) = L_max*2, 0;0, (L_max/2) 2:S. Pc(:, :, 1) = L_max2, 0。(L_max/2)2;case FS % Filling and Sloshing% State dimensionsd = 3;% Dynamic/process model% see Section 1.2.4 and equations (8) and (9) in document modelsA = zeros

18、 (sd, sd, T nmeas);w = 2*pi*sf; % omegafor nFl:T nmeasA(l, l,m) = 1；S Xp = zeros (sd, T.nmeas) : % predicted state (X)A (2, 2, m) = 1；A (3, 3, m)二1;S K二zeros (sd, T .nmeas); % Kalman gainA(l, 2, m) = dt;A(l, 2, m) = w*cos (w*T. ts (m) +sp*d2r);endQ = zeros (sd, sd, T nmeas);qcs = (sq-scale*sq-S)2; %

19、scale qc then square for varianceqcf = (sq_scale*sq_F)2; % scale qc then square for variance for ml:T.nmeasQ(l, l,m)=dt*(dt2+3*T. ts(m) 2+3*T. ts (m) *dt) (qcf+w2*cos (w*T. ts(m) *2*qcs)/3.0；Q(l, 2, m) = qcf*dt* (2*T. ts (m) + dt)/2. 0;Q(2, l,m) = Q(l,2, m);Q(l, 3, m) = w*cos(w*T. ts(m)*qcs*dt*(2*T.

20、 ts (m) +dt)/2. 0;Q(3, l,m) = Q(l,3,m);Q(2, 2, m) = qcf*dt;Q(3, 3, m) = qcs*dt;end% Set aside space for resultsS Xp = zeros(sd, T .nmeas); % predicted state (X)S Xc = zeros (sd, T .nmeas); % correctedS Pp = zeros (sd, sd, T .nmeas); % predicted covariance (P)S Pc = zeros (sd, sd, T .nmeas); % correc

21、tedS Zp = zeros(1, T .nmeas); % predicted measurement (Z)% Initialize filterL_init = L_max/2 0; % initial guessS. Xp(l, 1) = L_init;S. Xc(l, 1) = L_init;S Xp (3, 1) = T. sm; % temp - cheatS Xc (3, 1) = T. sm;S. Pp(:, 1) = L_max2, 0, 0; 0, (L_max/stime) 2, 0; 0, 0, (L_max/2) 2;S. Pc(:, :, 1) = L_max2

22、, 0, 0; 0, (L_max/stime) 2, 0; 0, 0, (L_max/2) 2;otherwiseerrorUnknown dynamic model type)；end% Choose the set of measurements based on the input selector cda(actual dynamics)switch cdacase,C% Constant level% Measurement modelif (mtype = 1)Za = T m l.C; % actual measurementselseZa = T.m.a.C; % actua

23、l measurementsendcase F % Filling level% Measurement modelif (mtype = 1)Za = T.m. l.F; % actual measurementselseZa = T.ni.a.F; % actual measurementsendcase S % Sloshing level% Measurement modelif (mtype = 1)Za = T m l S; % actual measurementselseZa = T.m.a.S; % actual measurementsendcase FS % Fillin

24、g and Sloshing level% Measurement modelif (mtype = 1)Za = T in. 1. FS; % actual measurementselseZa = T m a. FS; % actual measurementsendotherwiseerrorUnknown dynamic model type)；end% Measurement noise modelif (mtype = 1)% see Section 2. 1 in document modelsR二(srlm * kl)2;else% see Section 2 2 in doc

25、ument modelsR = (sra_d * ka)2;end% Loop through all measurementsfor k = 2:T nmeas% predictS. Xp(:, k) = A(:, :, k)*S.Xc(:, k-1);S. Pp(:，:, k) = A(:, :, k)*S.Pc(:, :, k-l)*A(:, :, k)+ Q(:, :, k);% measurement prediction and JacobianS Zp (k) = meas (S Xp (:, k), mtype, sd);H = measJacobian(S Xp(:，k), mtype, sd);% correctS. K(:, k)二S. Pp(:，:, k)*H*inv(H*S. Pp(:，:, k)*H + R) ; % Kalman gainS. Xc(:, k) = S.Xp(:, k) + S.K(:, k)*(Za(k)