Legend:
- Unmodified
- Added
- Removed
-
matlab/itsave.m
r7 r8 67 67 68 68 is_scalar = all(size(v) == 1); % true if scalar (for future use) 69 is_vector = (sum(size(v) > 1) <= 1); % true if a vector (or a scalar)70 is_intbin = min(min(v == round(v))); % true if integer or binary69 is_vector = 0;%(sum(size(v) > 1) <= 1); % true if a vector (or a scalar) 70 is_intbin = 0;%min(min(v == round(v))); % true if integer or binary 71 71 72 72 if ( isreal(v) && is_intbin ) % binary or integer type -
matlab/testKF.m
r7 r8 11 11 12 12 N =1000; 13 mu0 = [0;0]; 14 P0 = 200*eye(2); 13 15 14 16 u = ones(1,N); … … 24 26 25 27 d=[y;u]; 26 itsave('testKF.it',d) 28 itsave('testKF.it',d,A,B,C,D,Q,R,P0,mu0) 29 save testKF 27 30 else 28 31 %!cd ../testKF 29 32 33 load testKF 30 34 % init 31 mu = [0;0]; 32 P = 200*eye(2); 35 mu = mu0; 36 P = P0; 37 EP = [0;0]; 33 38 34 39 for t=2:N … … 41 46 K = P*C'*iRy; 42 47 P = P- K*C*P; % P = P -KCP; 48 EP(:,t) = eig(P); 49 if any(EP<0), keyboard; end 43 50 mu = mu + K*(y(t)-C*mu-D*u(t)); 44 51 Mu(1:2,t)=mu; … … 53 60 hold on 54 61 plot(xth','--'); 62 plot(xth2','+'); 55 63 plot(Mu',':'); 56 64 end