straihtflight_2d.m

二维冲突概率算法

%直线二维航迹冲突检测
function CP=StraihtFlight_2D(a,b,c,rate_h,rms_vp,Sc,  delta_Vx,delta_Vy,delta_Px0,delta_Py0)
%飞机1的起始位置（x1,y1）
%飞机2的起始位置（x2,y2）
%v1,v2两机地速
%a、b飞机1、2速度方向与x轴夹角
%c两航线夹角
%rate_h(两机相同) 飞机速度水平方向上的分量误差的线性增长因子，沿航迹方向的均方根rms_along = rate_h*t----沿航迹方向
%rms_vp(两机相同) 飞机在垂直方向上偏离航迹误差的均方根------垂直方向
%Sc两机安全距离
r =a*pi/180;
h =b*pi/180;
 if r>pi
        r=2*pi-r;
    else ;
 end
 if h>pi
        h=2*pi-h;
    else ;
 end
% 求预测时间t
t = -([delta_Px0,delta_Py0] * [delta_Vx;delta_Vy] / ([delta_Vx,delta_Vy] * [delta_Vx;delta_Vy]));
%坐标系转换矩阵
R1 = [cos(r),-sin(r);sin(r),cos(r)];
R2 = [cos(h),-sin(h);sin(h),cos(h)];
%两机互相独立的位置随机误差变量的协方差阵Cov1,Cov2
Cov1 = [(rate_h*t)^2, 0;
        0,rms_vp^2];
Cov2 = [(rate_h*t)^2, 0;
        0,rms_vp^2];
% 对两协方差阵进行坐标变换，转换至地球参考坐标系下；并合并误差协方差
Q1 = R1 * Cov1 * R1';
Q2 = R2 * Cov2 * R2';
M = Q1 + Q2;
% 坐标系变换，使椭圆概率空间域变换单位圆，求得变换矩阵R和T
% 对联合协方差阵进行Cholesky分解，使得M = LL'
L = (chol(M))';
delta_gamma = inv(L) * [delta_Vx;delta_Vy];
delta_gammax = delta_gamma(1);
delta_gammay = delta_gamma(2);
R = (1/(delta_gammax^2 + delta_gammay^2 )^0.5) * [ delta_gammax,  delta_gammay;
                                                   -delta_gammay, delta_gammax];
T = R * inv(L);

% 计算积分域（变换前为椭圆体积分域）边界
W = inv(T);
Params = W'*W;
r = Params(1);
s = Params(2);
z = Params(4);
delta_yc = Sc*(a/(r*z-s*s))^0.5;
% 原地球参考坐标系中最近点处两机的位置差（影响机一的联合误差区 和 机二的冲突区的 相对位置）
Delta_Pm = [delta_Px0;delta_Py0] + t * [delta_Vx;delta_Vy];
% 将位置差矢量进行T变换
Delta_Pm = T * Delta_Pm;
% 积分域上下限
y_min = -Delta_Pm(2) - delta_yc;
y_max = -Delta_Pm(2) + delta_yc;
% 计算冲突概率
CP = normcdf(y_max,0,1) - normcdf(y_min,0,1);