2n_ukf.rar_ sigma point_UKF_sigma point_卡尔曼 弹道 matlab_卡尔曼滤波 比较

function main_2n_ukf() clc; clear all; close all; load reference; % Given data x0 = [300000; -20000; 0.001]; P = [1000000 0 0;0 4000000 0;0 0 10]; xe = x0; %w=[100;10;0.0001]; %w=[10;1;0.00001]; %w=[1;0.1;0.00001]; %w=[0.1;0.01;0.000001]; %w=[0.01;0.001;0.0000001]; w=[0.001;0.0001;0.00000001]; Q = [w(1)^2 0 0;0 w(2)^2 0;0 0 w(3)^2]; dt = 0.01; R =100000; u0 = [0;0;0]; % Filter loop N = 3000; for i=1:N % add noise to measurments y = multivariate_gauss_noise(Yture(i), R, 1); u = multivariate_gauss_noise(u0, Q, 1); % predict (time update equations) [xe,P] = predict(xe,P,u,Q,dt); % update (measurement update equations) [xe,ye,P] = update(xe,y,P,R); % plots X(:,i)=xe; Y(i)=ye; end s2n_ukf_errorX=X-Xture; s2n_ukf_errorY=Y-Yture; figure(); subplot(3,1,1); plot(X(1,:),'k-'); xlabel('time(10ms)'),ylabel('altitude(ft)');title('2n ukf estimated altitude'); hold on; subplot(3,1,2); plot(X(2,:),'b-'); xlabel('time(10ms)'),ylabel('velocity(ft/s)');title('2n ukf estimated velocity'); hold on; subplot(3,1,3); plot(X(3,:),'r-'); xlabel('time(10ms)'),ylabel('ball.coeff.');title('2n ukf estimated ball.coeff.'); hold on; figure(); subplot(3,1,1); plot(s2n_ukf_errorX(1,:),'k-'); xlabel('time(10ms)'),ylabel('altitude error(ft)');title('2n ukf estimated altitude error'); hold on; subplot(3,1,2); plot(s2n_ukf_errorX(2,:),'b-'); xlabel('time(10ms)'),ylabel('velocity error(ft/s)');title('2n ukf estimated velocity error'); hold on; subplot(3,1,3); plot(s2n_ukf_errorX(3,:),'r-'); xlabel('time(10ms)'),ylabel('ball.coeff. error');title('2n ukf estimated ball.coeff. error'); hold on; save 2n_ukf_error s2n_ukf_errorX s2n_ukf_errorY; function [xe,P] = predict(xe,P,u,Q,dt) xe = [xe; u]; P = blkdiag(P,Q); [xe,P] = unscented_transform(@predict_model,[],xe,P,dt); function [xe,ye,P] = update(xe,y,P,R) [xe,ye,P] = unscented_update(@observe_model,@observe_residual,xe,P,y,R); function x = predict_model(x,dt) Rho = 2; g = 32.2; k = 20000; x = [x(1,:)+x(2,:)*dt+x(4,:); x(2,:)+dt*Rho*exp(-x(1,:)/k).*(x(2,:).^2).*x(3,:)/2-dt*g+x(5,:); x(3,:)+x(6,:)]; function y = observe_model(x) M = 100000; a = 100000; temp1=size(x); n=temp1(2); M=repvec(M,n); a=repvec(a,n); y = sqrt(M.^2+(x(1,:)-a).^2); function x = repvec(x,N) x = x(:, ones(1,N)); function v = observe_residual(z1, z2) % Given two observation values, compute their normalised residual. % Notice, once again, that the model is vectorised in terms of z1 and z2. v = z1-z2;