请教一个飞机航迹建模还有跟踪的程序

[baidu_38296159]

1、clear%%清空变量环境
clc
load shuju %%%航班航迹数据

Re = 6371393;
%真实轨迹
a_R = yout(:,1:3);
v_R = yout(:,4:6);
p_R = yout(:,7:9);
%加噪声后的航迹计算结果
a_ins = yout(:,10:12);
v_ins = yout(:,13:15);
p_ins = yout(:,16:18);
quat = yout(:,19:22); %姿态四元数
Fn = yout(:,23:25); %地理系下的比力

%惯导相关的噪声统计数据
Q_wg = (0.04/(57*3600))^2; %陀螺马氏过程
Q_wr = (0.01/(57*3600))^2; %陀螺白噪声
Q_wa = (1e-3)^2; %加计马氏过程
Q = diag([Q_wg Q_wg Q_wg, Q_wr Q_wr Q_wr, Q_wa Q_wa Q_wa]);
Tg = 300*ones(3,1);
Ta = 1000*ones(3,1);

%得到带误差的GPS输出信号
p_gps_sample = p_R(1:10:end,:);
n = size(p_gps_sample,1);
p_error(:,1:2) = 30*randn(n,2)/Re;
p_error(:,3) = 30*randn(n,1); %位置误差
p_gps = p_gps_sample+p_error; %加入位置误差
R = diag(std(p_error).^2); %计算测量噪声方差R

%卡尔曼滤波
tao = 1; %滤波步长
a_ins_sample = a_ins(1:10:end,:);
v_ins_sample = v_ins(1:10:end,:);
p_ins_sample = p_ins(1:10:end,:);
a_R_sample = a_R(1:10:end,:);
v_R_sample = v_R(1:10:end,:);
p_R_sample = p_R(1:10:end,:);
Dp = p_ins_sample-p_gps; %INS与GPS下输出的位置差值
a = a_ins_sample;
v = v_ins_sample;
p = p_ins_sample;
quat0 = quat(1:10:end,:);
Fn0 = Fn(1:10:end,:);
Error_before = p_R_sample-p*6e6;

[Error_a, Error_v, Error_p, PP] = weizhi(Dp, v, p, quat0, Fn0, Q, R, Tg, Ta, tao); %得到位置，速度误差误差估计值

a_estimate = a(1:size(Error_a,1),:)-Error_a;
v_estimate = v(1:size(Error_v,1),:)-Error_v;
p_estimate = p(1:size(Error_p,1),:)-Error_p;

n = size(p_estimate,1);

%位置误差比较
figure
subplot(3,1,1)
plot((1:n),(p_R_sample(1:n,1)-p(1:n,1))*6e6,'k',(1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6e6,'r') %黑线-滤波前的误差 红线-滤波后的误差
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),(p_R_sample(1:n,2)-p(1:n,2))*6e6,'k',(1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6e6,'r') %黑线-滤波前的误差 红线-滤波后的误差
ylabel('位置误差，单位m')
subplot(3,1,3)
plot((1:n),p_R_sample(1:n,3)-p(1:n,3),'k',(1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r') %黑线-滤波前的误差 红线-滤波后的误差
xlabel('黑线-滤波前的位置误差 红线-滤波后位置误差')

%速度误差比较
figure
subplot(3,1,1)
plot((1:n),v_R_sample(1:n,1)-v(1:n,1),'k',(1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),v_R_sample(1:n,2)-v(1:n,2),'k',(1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')
ylabel('速度误差，单位m/s')
subplot(3,1,3)
plot((1:n),v_R_sample(1:n,3)-v(1:n,3),'k',(1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')
xlabel('黑线-滤波前速度误差 红线-滤波后速度误差')

%位置误差
figure
subplot(3,1,1)
xlabel('时间，单位s')
plot((1:n),(p_R_sample(1:n,1)-p_estimate(:,1))*6370000,'r') %
subplot(3,1,2)
plot((1:n),(p_R_sample(1:n,2)-p_estimate(:,2))*6370000,'r') %
ylabel('位置误差，单位m')
subplot(3,1,3)
plot((1:n),p_R_sample(1:n,3)-p_estimate(:,3),'r')
xlabel('滤波后的位置误差')

%速度误差
figure
subplot(3,1,1)
plot((1:n),v_R_sample(1:n,1)-v_estimate(:,1),'r')
xlabel('时间，单位s')
subplot(3,1,2)
plot((1:n),v_R_sample(1:n,2)-v_estimate(:,2),'r')
ylabel('速度误差，单位m/s')
subplot(3,1,3)
plot((1:n),v_R_sample(1:n,3)-v_estimate(:,3),'r')
xlabel('滤波后的速度误差')

2、clc;
clear;
T=1;%雷达扫描周期
N=80/T;%总采样次数
X=zeros(4,N);%目标的真实位置和速度
X(:,1)=[0,20,0,45];%目标的初始位置
Z=zeros(2,N);%位置的观测
Z(:,1)=[X(1,1),X(3,1)];
delta_w=1e-2;%过程噪声
Q=delta_w*diag([0.5,1,0.5,1]);%
R=100*eye(2);%观测噪声均值
F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
H=[1,0,0,0;0,0,1,0];

for t=2:N
X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);%目标真实轨迹
Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);%对目标的观测
end

%Kalman Filter
Xkf=zeros(4,N);
Xkf(:,1)=X(:,1);%Kalman滤波状态初始化
P0=eye(4);%协方差矩阵初始化
for i=2:N
Xn=F*Xkf(:,i-1);%预测
P1=F*P0*F'+Q;%观测误差协方差
K=P1*H'*inv(H*P1*H'+R);%增益
Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);%状态更新
P0=(eye(4)-K*H)*P1;%滤波误差协方差更新
end

%%误差分析
for i=1:N
Err_Observation(i)=RMS(X(:,i),Z(:,i));
Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
end

figure
hold on; box on;
plot(X(1,:),X(3,:),'-k');
plot(Z(1,:),Z(2,:),'-b');
plot(Xkf(1,:),Xkf(3,:),'-r');
legend('真实轨迹','观测轨迹','滤波轨迹');
xlabel('横坐标/m');
ylabel('纵坐标/m');

figure
hold on; box on;
plot(Err_Observation,'','MarkerFace','g');
plot(Err_KalmanFilter,'','MarkerFace','r');
legend('滤波前的误差','滤波后的误差');
xlabel('观测时间/s');
ylabel('误差');


这个程序是GPS和INS组合系统的，程序的跟踪用卡尔曼滤波，这两个matlab程序如果要加入其他的不确定性因素，比如风速、飞行员意图、陀螺仪的随机误差，应该怎么写代码呢，有大佬知道吗，我刚刚接触matlab，感觉不太懂，求助