基于Matlab模拟分配模型仿真（源码+数据）.rar

function out = EKF_dynamic(u) global miu; global Pk; global Xk; global dt; global tt; r_real = u(1:3); t = u(4); Q = zeros(6); %%%%%%%%辅助公式%%%%% % X(k+1/k) = X(k) + f(X(k))*T + A(X(k))*f(X(k))*T^2/2; % P(k+1/k) = fai(k)*P(k)*fai(k)' + Q(k); % K(k+1/k) = p(k+1/k)*H(k+1)'*inv( H(k+1)*P(k+1/k)*H(k+1)' + R(k+1) ); % X(k+1) = X(k+1/k) + K(k+1)*[ Z(k+1) - h(X(k+1/k)) ]; % P(k+1) = [ I - K(k+1)*H(k+1) ]*P(k+1/k); %%%%%%%%导航部分%%%%% if abs(t - dt*round(t/dt)) < 0.01 if abs(t-tt)>0.02 tt = t; f = 0.2; %%%%%%%%观测值%%%%%%% h = [1;0;0]; zb = r_real/norm(r_real); yb = cross(r_real/norm(r_real),h/norm(h))/norm(cross(r_real/norm(r_real),h/norm(h))); xb = cross(yb,zb); Cba = [xb';yb';zb']; r_real = Cba*r_real; px = f*(r_real(1)/r_real(3) + 18/3600*pi/180*randn); py = f*(r_real(2)/r_real(3) + 18/3600*pi/180*randn); Rk = [(f*18/3600*pi/180)^2 0 0 (f*18/3600*pi/180)^2]; %%%%%%%%状态转换矩阵%%%%%%% rxk = Xk(1); ryk = Xk(2); rzk = Xk(3); vxk = Xk(4); vyk = Xk(5); vzk = Xk(6); r_norm = sqrt(rxk^2 + ryk^2 + rzk^2); A1 = miu*[-1/r_norm^3+3*rxk^2/r_norm^5 3*rxk*ryk/r_norm^5 3*rxk*rzk/r_norm^5; 3*rxk*ryk/r_norm^5 -1/r_norm^3+3*ryk^2/r_norm^5 3*ryk*rzk/r_norm^5; 3*rxk*rzk/r_norm^5 3*ryk*rzk/r_norm^5 -1/r_norm^3+3*rzk^2/r_norm^5]; A = [zeros(3) eye(3); A1 zeros(3)]; fai = eye(6) + A*dt + A^2*dt/2; %%%%%%%%预测值%%%%%%% option = odeset('RelTol', 1e-4); tspan = [0 dt]; [t_orbit, x_orbit] = ode45('dif_orbit_dynamics',tspan,Xk,option); Xk1 = x_orbit(end,:)'; Pk1k = fai*Pk*fai'+ Q; %%%%测量预测 r_p = Cba*Xk1(1:3); px_p = f*(r_p(1)/r_p(3)); py_p = f*(r_p(2)/r_p(3)); H1 = [f/r_p(3) 0 -f*r_p(1)/r_p(3)^2; 0 f/r_p(3) -f*r_p(2)/r_p(3)^2]; H = [H1*Cba zeros(2,3)]; %%%%%%%%滤波器%%%%%%% Kk = Pk1k*H'/(H*Pk1k*H'+Rk); Xk = Xk1 + Kk*[px-px_p; py-py_p]; Pk = (eye(6)-Kk*H)*Pk1k; end out = Xk; else dett = t - dt*fix(t/dt); out = [(Xk(1:3)+dett*Xk(4:6));Xk(4:6)]; end end