kalman_filter.rar_卡尔曼滤波实现

clc; clear; close all; al=40*pi/180; % angle of velocity at the bigining v0=10; % initial velocity g=9.8; % gravitation constant vx=v0*cos(al); vy=v0*sin(al); % initial velocities x=0; y=0; % initial position dt=0.02; % time step sgx=0.02; % sigma_x - nose lavel for x sgy=0.02; % sigma_y - nose lavel for y % generate original trajectory: xa=[]; % here x coordinaties will be stored ya=[]; % here y coordinaties will be stored vxa=[]; % here x-component of velocity will be stored vya=[]; % here y-component of velocity will be stored ta=[]; % here times will be stored xna=[]; % here noised x coordinaties will be stored yna=[]; % here noised y coordinaties will be stored t=0; % start time while 1 % infinite loop, break used xa=[xa x]; ya=[ya y]; vxa=[vxa vx]; vya=[vya vy]; ta=[ta t]; xna=[xna x+sgx*randn]; yna=[yna y+sgy*randn]; % add gaussian noise if y<0 % when return to ground break % stop motion end % update values x=x+vx*dt; % vx~(x(k+1)-x(k))/dt => x(k+1)~x(k)+vx*dt y=y+vy*dt; vy=vy-g*dt; t=t+dt; % note: vx not change end % plot nosied trajectory %plot(xa,ya,'b-',xna,yna,'r--'); figure; plot(xna,yna,'b-'); xlabel('x, m'); ylabel('y, m'); %legend('real curve','noised curve'); % apply kalman filtering: R=[sgx^2 0 0 sgy^2]; % covariance matrix for nose in x and y H=[1 0 0 0 0; 0 0 1 0 0]; % observarion matrix F=[ 1 dt 0 0 0 0 1 0 0 0 0 0 1 dt 0 0 0 0 1 dt 0 0 0 0 1 ]; % state transition matrix P=eye(5); % inital gues for P-matrix I=eye(5); % unit matrix % estimated state vector form: % xc =[ x % vx % y % vy % ay ] xc=[xna(1) 0 yna(1) 0 0]; % guess for initial value of estimation of state vector % for x y get initial noised position, for vx vy ay is zeros z=[xna yna]; % observation xca=xc; % will be stored in xca nc=2; % loop counter, stert from second step for t=ta(2:end) Ps=F*P*F'; K=Ps*H'*(H*Ps*H'+R)^(-1); xc=F*xc+K*(z(:,nc)-H*F*xc); P=(I-K*H)*Ps*(I-K*H)'+K*R*K'; %P=(I-K*H)*Ps; xca=[xca xc]; % store nc=nc+1; end % add to plot filtered tragectory: hold on; plot(xca(1,:),xca(3,:),'r-','linewidth',2); legend('noised','filtered'); title('trajectory'); % plot velocities graph in separated window: figure; plot(ta,xca(2,:),'r-','linewidth',2); hold on; plot(ta,vxa,'r--'); plot(ta,xca(4,:),'b-','linewidth',2); plot(ta,vya,'b--'); xlabel('time, s'); legend('filtered vx','original vx','filtered vy','original vy'); title('velocities');