XT7_45_b.zip_加速度 观测_卡尔曼滤波_卡尔曼滤波拓展_相对观测_相对运动

%% % name:XT7_45_b % explain:example 7.8.3 的拓展，加入速度观测和加速度观测 %% function XT7_45_b clc close all clear all %--------------- Target and sensor models------------------- T = 0.1; alpha = 0.9; A = [1 T T*T/2; 0 1 T; 0 0 alpha]; B = [0;0;1]; vareta = 0.25; R_eta = vareta; H = [1 0 0]; D =eye(1); varv = 0.25; Rv = diag(varv); % --------Generate State and Observation signals-------------- Nt = 100; eta = sqrt(vareta)*randn(1,Nt); y = zeros(3,Nt); y(:,1) = A*[0;1;0] + B*eta(1,1); for n = 2:Nt y(:,n) = A*y(:,n-1) + B*eta(1,n); end v = sqrt(varv)*randn(1,Nt); x = zeros(1,Nt); for n = 1:Nt x(:,n) = H*y(:,n) + D*v(:,n); end %--------------------- A-priori estimates----------------------------- yhat_ini = [0;0;0]; R_y = 2*eye(3); %------------------- Kalman Filter------------------------------------ [y_hat,GainK,Re_pre,Re_post] = skf(A,B,R_eta,H,Rv,x,yhat_ini,R_y); %----------------Extract quantities for plotting---------------------- yp = y(1,2:end);% True position yv = y(2,2:end);% True velocity ya = y(3,2:end);% True acceleration xp = x(1,2:end); % Noisy position yp_hat = y_hat(1,2:end); % Estimated position yv_hat = y_hat(2,2:end); % Estimated velocity ya_hat = y_hat(3,2:end); % Estimated acceleration Kg1 = GainK(1,1,2:end); Kg1 = Kg1(:);% Kalman Gain (1,1) Kg2 = GainK(2,1,2:end); Kg2 =Kg2(:);% Kalman Gain (2,2) Kg3 = GainK(3,1,2:end); Kg3 =Kg3(:);% Kalman Gain (2,2) Rpre = zeros(1,100); Rpost = zeros(1,100); for n = 1:Nt Rpre(n) = trace(Re_pre(:,:,n)); Rpost(n) = trace(Re_post(:,:,n)); end %-------------------------- Plotting-------------------------------- time = (2:Nt)*T; figure fsize = 10; Lwide = 2; subplot(3,1,1); plot(time,yp,'g',time,xp,'r:',time,yp_hat,'b--','LineWidth',Lwide); % axis([time(1),time(end),-5,15]); ylabel('Position (meters)','fontsize',fsize); title('True, Noisy, and Estimated Positions','fontsize',fsize); legend('True','Noisy','Estimate'); set(gca,'fontsize',fsize) subplot(3,1,2); plot(time,yv,'g',time,yv_hat,'b--','LineWidth',Lwide); % axis([0,10,-2,4]); xlabel('t (sec)','fontsize',fsize); ylabel('velocity (m/sec)','fontsize',fsize); legend('True','Noisy','Estimate'); title('True and Estimated Velocities','fontsize',fsize); set(gca,'fontsize',fsize) subplot(3,1,3); plot(time,ya,'g',time,ya_hat,'b--','LineWidth',Lwide); % axis([0,10,-2,4]); xlabel('t (sec)','fontsize',fsize); ylabel('velocity (m/sec)','fontsize',fsize); legend('True','Noisy','Estimate'); title('True and Estimated Accelerations','fontsize',fsize); set(gca,'fontsize',fsize) figure subplot(2,1,1); mrsize = 5; plot(time,Kg1,'bo',time,Kg2,'r.',time,Kg3,'g*','markersize',mrsize); axis([0,10,0,1]); xlabel('t (sec)','fontsize',fsize); legend('K_p','K_v',1) title('Kalman gain components','fontsize',fsize); set(gca,'fontsize',fsize) subplot(2,2,3); plot(time(2:41),Rpre(1:40),'bo',time(2:41),Rpost(1:40),'r.','markersize',mrsize); % axis([0,4.0,0,1]); xlabel('t (sec)','fontsize',fsize); legend('a-priori','a-posteriori'); title('Trace of covariance matrix','fontsize',10); set(gca,'fontsize',fsize) subplot(2,2,4); plot(time(61:72),Rpre(60:71),'bo',time(61:72),Rpost(60:71),'r.','markersize',mrsize); hold on; timenew = [1;1]*time(61:72); timenew = (timenew(:))'; tpmtp = zeros(1,23); tpmtp(1:2:23) = Rpre(60:71); tpmtp(2:2:24) = Rpost(60:71); plot(timenew,tpmtp,'g:'); hold off; % axis([5.95,7.05,0.04,0.05]); xlabel('t (sec)','fontsize',fsize); legend('a-priori','a-posteriori'); title('Trace of covariance matrix','fontsize',fsize); set(gca,'fontsize',fsize) end function [y_hat,GainK,Re_pre,Re_post] = skf(A,B,R_eta,H,Rv,x,yhat_ini,R_y) N = length(x); [M]= size(A,2); [K]= size(H,1); y_hat = zeros(M,N) ; y_hat(:,1) = yhat_ini ; Re_post= zeros(M,M,N); Re_post(:,:,1) = R_y; Re_pre= zeros(M,M,N); GainK =zeros(M,K,N); for i = 2:N yhat(:,i) = A*y_hat(:,i-1); xhat = H*yhat(:,i); Re_pre(:,:,i) = A*Re_post(:,:,i-1) *A' + B*R_eta*B'; Rw = H*Re_pre(:,:,i)*H' + Rv; GainK(:,:,i) = Re_pre(:,:,i)*H'*inv(Rw); y_hat(:,i) = yhat(:,i) + GainK(:,:,i) *(x(:,i) - xhat); Re_post(:,:,i) = Re_pre(:,:,i) - GainK(:,:,i) *H*Re_pre(:,:,i); end end