Legend:
- Unmodified
- Added
- Removed
-
matlab/testKF.m
r33 r37 5 5 A=[1 -0.5; 1 0]; 6 6 B=[1;0.1]; 7 C=[1 0 ; 0 1];8 D= [0.1; 0];9 R= [1 0; 0 0.1];7 C=[1 0];%; 0 1]; 8 D=0.1;%[0.1; 0]; 9 R=0.1;%[1 0; 0 0.1]; 10 10 Q=[0.2 0 ; 0 0.2]; 11 11 … … 13 13 sR = chol(R)'; 14 14 15 N = 1000;15 N =3000; 16 16 mu0 = [0;0]; 17 17 P0 = 200*eye(2); 18 18 19 u = rand(1,N);19 u = zeros(1,N); 20 20 x = zeros(2,N); 21 y = zeros( 2,N);21 y = zeros(1,N); 22 22 23 23 x(:,1) = [10;10]; 24 24 Et = sQ*randn(2,N); 25 Wt = sR*randn( 2,N);25 Wt = sR*randn(1,N); 26 26 for i=2:N; 27 27 x(:,i) = A*x(:,i-1) + B*u(i) + Et(:,i); … … 43 43 OPt = P0; 44 44 ll =0; 45 Oxt2= mu0;46 OPt2 = P0;47 ll2=0;45 oxt = mu0; 46 oPt = chol(P0)'; 47 oll=0; 48 48 49 tic; 50 for t=2:N 51 % mu = A*mu + B*u(t); 52 % P = A*P*A' + Q; 53 % 54 % %Data update 55 % Ry = C*P*C' + R; 56 % iRy = inv(Ry); 57 % K = P*C'*iRy; 58 % P = P- K*C*P; % P = P -KCP; 59 % mu = mu + K*(y(:,t)-C*mu-D*u(t)); 60 % Mu(1:2,t)=mu; 49 61 50 for t=2:N 51 mu = A*mu + B*u(t); 52 P = A*P*A' + Q; 53 54 %Data update 55 Ry = C*P*C' + R; 56 iRy = inv(Ry); 57 K = P*C'*iRy; 58 P = P- K*C*P; % P = P -KCP; 59 mu = mu + K*(y(t)-C*mu-D*u(t)); 60 Mu(1:2,t)=mu; 61 62 [Oxt,OPt,ll(t)] = Kalman(Oxt,y(t),A,C,Q,R,OPt); 63 [Oxt2,OPt2,ll2(t)] = Kalman(Oxt2,y(t),A,C,Q,R/10000,OPt2); 62 [Oxt,OPt,ll(t)] = Kalman(Oxt,y(:,t),A,C,Q,R,OPt); 63 % [oxt,oPt,oll(t)] = KalmanSq(oxt,y(:,t),A,C,sQ,sR,oPt); 64 MuK(1:2,t) = Oxt; 65 % MuS(1:2,t) = oxt; 64 66 end 65 66 keyboard67 exec_matlab = toc 68 %keyboard 67 69 68 70 !cd ../;./tests/testKF … … 72 74 plot(x'); 73 75 hold on 74 plot( xth','--');76 plot([zeros(size(xth,1),1) xth]','--'); % shift the predictions 75 77 plot(xth2','+'); 76 78 plot(xthE','o'); 77 plot(Mu',':'); 78 keyboard 79 plot([zeros(size(xth,1),1) MuK]','d'); % shift the predictions 80 81 exec_times 82 exec_matlab./exec_times 79 83 end
