Постановка задачи

Для того чтобы определить вероятные положения летательного аппарата в окрестностях траектории необходимо использовать комплексную обработку данных полученных с различных источников, в рамках данной статьи предполагается что в основу расчета берем усредненные параметры участка траектории ЛА, известные координаты РЛС которые определяют его положение, дисперсии для каждой РЛС (в рамках данного моделирования берем две, но в произвольном случае может быть любое количество)

Подобные расчеты требуются для того чтобы определить как близко могут пролететь самолеты один относительно другого в сложных навигационных условиях (например в условиях заглушенного сигнала GPS), область вероятного положения в каждый момент времени при движении летательного аппарата будет представлять собой серию эллипсоидов, параметры данных эллипсоидов будут вычисляться с помощью скрипта на языке Engee

Синтез траектории движения

В рамках данного расчета траекторию зададим как линейную пространственную

h = 0.1; tk = 0:h:100; t0 = tk[1]; A = [0 5 0 5 10 5 0 5 7]; X = zeros(1,length(tk)); Y = zeros(1,length(tk)); Z = zeros(1,length(tk)); G = zeros(9,9); H = zeros(length(tk),9,6) K_out = zeros(length(tk),9,9) x(t) = A[1] + A[4]*(t-t0); y(t) = A[2] + A[5]*(t-t0); z(t) = A[3] + A[6]*(t-t0); for j = 1:4 X[j] = x(tk[j]) Y[j] = y(tk[j]) Z[j] = z(tk[j]) end

Параметры РЛС

Для данного скрипта требуется задать точки в которых находятся РЛС в декартовой системе координат и дисперсии для измеряемых координат

# координаты РЛС в виде [x y z]

coord1 = [200 600 0];

coord2 = [200 200 0]; ## матрица дисперсий для измерителей в виде [погрешность линейная, погрешность угловая 1, погрешность угловая 2]

D = [1; 5/180*pi; 5/180*pi; 1; 5/180*pi; 5/180*pi];

Матрица соответствия для параметров траектории

Матрица соответствия (часто называемая матрицей чувствительности или матрицей идентификации) в контексте оценивания параметров траектории — это матрица, которая связывает небольшие изменения в параметрах модели с соответствующими изменениями в наблюдаемых или прогнозируемых величинах траектории.

# матрица соответствия F(t) = [1 0 0 (t-t0) 0 0 (t-t0)^2 0 0; 0 1 0 0 (t-t0) 0 0 (t-t0)^2 0; 0 0 1 0 0 (t-t0) 0 0 (t-t0)^2 0 0 0 1 0 0 (t-t0)*2 0 0; 0 0 0 0 1 0 0 (t-t0)*2 0; 0 0 0 0 0 1 0 0 (t-t0)*2; 0 0 0 0 0 0 2 0 0; 0 0 0 0 0 0 0 2 0; 0 0 0 0 0 0 0 0 2];

Вспомогательные функции для матрицы Якоби

радиус - вектора от РЛС до усредненных точек траектории

r1(t) = sqrt((x(t)-coord1[1])^2+(y(t)-coord1[2])^2+(z(t)-coord1[3])^2); # радиус вектор до объекта rg1(t) = sqrt((x(t)-coord1[1])^2+(z(t)-coord1[3])^2); # радиус вектор (проекция на плоскость) r2(t) = sqrt((x(t)-coord2[1])^2+(y(t)-coord2[2])^2+(z(t)-coord2[3])^2); # радиус вектор до объекта rg2(t) = sqrt((x(t)-coord2[1])^2+(z(t)-coord2[3])^2); # радиус вектор (проекция на плоскость)

Коэффициенты матрицы Якоби для первого РЛС

# первая строка матрицы Якоби f11(t)= (x(t)-coord1[1])/r1(t); f12(t)= (y(t)-coord1[2])/r1(t); f13(t)= (z(t)-coord1[3])/r1(t); f14(t)= ((x(t)-coord1[1])/r1(t))*(t-t0); f15(t)= ((y(t)-coord1[2])/r1(t))*(t-t0); f16(t)= ((z(t)-coord1[3])/r1(t))*(t-t0); f17(t)= ((x(t)-coord1[1])/r1(t))*(t-t0)^2; f18(t)= ((y(t)-coord1[2])/r1(t))*(t-t0)^2; f19(t)= ((z(t)-coord1[3])/r1(t))*(t-t0)^2; # вторая строка матрицы Якоби f21(t)= -(z(t)-coord1[3])/(rg1(t))^2; f22(t)= 0; f23(t)= (x(t)-coord1[1])/(rg1(t))^2; f24(t)= -(z(t)-coord1[3])/(rg1(t))^2*(t-t0); f25(t)= 0; f26(t)= (x(t)-coord1[1])/(rg1(t))^2*(t-t0); f27(t)= -(z(t)-coord1[3])/(rg1(t))^2*(t-t0)^2; f28(t)= 0; f29(t)= (x(t)-coord1[1])/(rg1(t))^2*(t-t0)^2; # третья строка матрицы Якоби f31(t)= -1/(rg1(t))*(x(t)-coord1[1])/r1(t)*(y(t)-coord1[2])/r1(t); f32(t)= rg1(t)/(r1(t)^2); f33(t)= -1/(rg1(t))*(y(t)-coord1[2])/r1(t)*(z(t)-coord1[3])/r1(t); f34(t)= -1/(rg1(t))*(x(t)-coord1[1])/r1(t)*(y(t)-coord1[2])/r1(t)*(t-t0); f35(t)= rg1(t)/(r1(t)^2)*(t-t0); f36(t)= -1/(rg1(t))*(y(t)-coord1[2])/r1(t)*(z(t)-coord1[3])/r1(t)*(t-t0); f37(t)= -1/(rg1(t))*(x(t)-coord1[1])/r1(t)*(y(t)-coord1[2])/r1(t)*(t-t0)^2; f38(t)= rg1(t)/(r1(t)^2)*(t-t0)^2; f39(t)= -1/(rg1(t))*(y(t)-coord1[2])/r1(t)*(z(t)-coord1[3])/r1(t)*(t-t0)^2;

Коэффициенты матрицы Якоби для второго РЛС

# второй измеритель # первая строка матрицы Якоби f11_2(t)= (x(t)-coord2[1])/r2(t); f12_2(t)= (y(t)-coord2[2])/r2(t); f13_2(t)= (z(t)-coord2[3])/r2(t); f14_2(t)= ((x(t)-coord2[1])/r2(t))*(t-t0); f15_2(t)= ((y(t)-coord2[2])/r2(t))*(t-t0); f16_2(t)= ((z(t)-coord2[3])/r2(t))*(t-t0); f17_2(t)= ((x(t)-coord2[1])/r2(t))*(t-t0)^2; f18_2(t)= ((y(t)-coord2[2])/r2(t))*(t-t0)^2; f19_2(t)= ((z(t)-coord2[3])/r2(t))*(t-t0)^2; # вторая строка матрицы Якоби f21_2(t)= -(z(t)-coord2[3])/(rg2(t))^2; f22_2(t)= 0; f23_2(t)= (x(t)-coord2[1])/(rg2(t))^2; f24_2(t)= -(z(t)-coord2[3])/(rg2(t))^2*(t-t0); f25_2(t)= 0; f26_2(t)= (x(t)-coord2[1])/(rg2(t))^2*(t-t0); f27_2(t)= -(z(t)-coord2[3])/(rg2(t))^2*(t-t0)^2; f28_2(t)= 0; f29_2(t)= (x(t)-coord2[1])/(rg2(t))^2*(t-t0)^2; # третья строка матрицы Якоби f31_2(t)= -1/(rg2(t))*(x(t)-coord2[1])/r2(t)*(y(t)-coord2[2])/r2(t); f32_2(t)= rg2(t)/(r2(t)^2); f33_2(t)= -1/(rg2(t))*(y(t)-coord2[2])/r2(t)*(z(t)-coord2[3])/r2(t); f34_2(t)= -1/(rg2(t))*(x(t)-coord2[1])/r2(t)*(y(t)-coord2[2])/r2(t)*(t-t0); f35_2(t)= rg2(t)/(r2(t)^2)*(t-t0); f36_2(t)= -1/(rg2(t))*(y(t)-coord2[2])/r2(t)*(z(t)-coord2[3])/r2(t)*(t-t0); f37_2(t)= -1/(rg2(t))*(x(t)-coord2[1])/r2(t)*(y(t)-coord2[2])/r2(t)*(t-t0)^2; f38_2(t)= rg2(t)/(r2(t)^2)*(t-t0)^2; f39_2(t)= -1/(rg2(t))*(y(t)-coord2[2])/r2(t)*(z(t)-coord2[3])/r2(t)*(t-t0)^2;

Матрица Якоби для двух РЛС

Матрица Якоби содержит частные производные наблюдаемых величин или выходов системы по отношению к параметрам. Она показывает, насколько сильно меняется модельная траектория при небольших изменениях параметров. Наличие полноразмерной, невырожденной матрицы Якоби свидетельствует об хорошей идентифицируемости и возможности точной оценки параметров.

## матрица частных производных (Якоби) H0(t)= [f11(t) f12(t) f13(t) f14(t) f15(t) f16(t) f17(t) f18(t) f19(t); f21(t) f22(t) f23(t) f24(t) f25(t) f26(t) f27(t) f28(t) f29(t); f31(t) f32(t) f33(t) f34(t) f35(t) f36(t) f37(t) f38(t) f39(t); f11_2(t) f12_2(t) f13_2(t) f14_2(t) f15_2(t) f16_2(t) f17_2(t) f18_2(t) f19_2(t); f21_2(t) f22_2(t) f23_2(t) f24_2(t) f25_2(t) f26_2(t) f27_2(t) f28_2(t) f29_2(t); f31_2(t) f32_2(t) f33_2(t) f34_2(t) f35_2(t) f36_2(t) f37_2(t) f38_2(t) f39_2(t)];

Далее необходимо для каждого момента времени рассчитать значения матрицы Якоби, таким образом получаем трехмерную матрицу

for i = 1:length(tk) H[i,1,1] = f11(tk[i]) H[i,2,1] = f12(tk[i]); H[i,3,1] = f13(tk[i]); H[i,4,1] = f14(tk[i]); H[i,5,1] = f15(tk[i]); H[i,6,1] = f16(tk[i]); H[i,7,1] = f17(tk[i]); H[i,8,1] = f18(tk[i]); H[i,9,1] = f19(tk[i]); H[i,1,2] = f21(tk[i]); H[i,2,2] = f22(tk[i]); H[i,3,2] = f23(tk[i]); H[i,4,2] = f24(tk[i]); H[i,5,2] = f25(tk[i]); H[i,6,2] = f26(tk[i]); H[i,7,2] = f27(tk[i]); H[i,8,2] = f28(tk[i]); H[i,9,2] = f29(tk[i]); H[i,1,3] = f31(tk[i]); H[i,2,3] = f32(tk[i]); H[i,3,3] = f33(tk[i]); H[i,4,3] = f34(tk[i]); H[i,5,3] = f35(tk[i]); H[i,6,3] = f36(tk[i]); H[i,7,3] = f37(tk[i]); H[i,8,3] = f38(tk[i]); H[i,9,3] = f39(tk[i]); H[i,1,4] = f11_2(tk[i]); H[i,2,4] = f12_2(tk[i]); H[i,3,4] = f13_2(tk[i]); H[i,4,4] = f14_2(tk[i]); H[i,5,4] = f15_2(tk[i]); H[i,6,4] = f16_2(tk[i]); H[i,7,4] = f17_2(tk[i]); H[i,8,4] = f18_2(tk[i]); H[i,9,4] = f19_2(tk[i]); H[i,1,5] = f21_2(tk[i]); H[i,2,5] = f22_2(tk[i]); H[i,3,5] = f23_2(tk[i]); H[i,4,5] = f24_2(tk[i]); H[i,5,5] = f25_2(tk[i]); H[i,6,5] = f26_2(tk[i]); H[i,7,5] = f27_2(tk[i]); H[i,8,5] = f28_2(tk[i]); H[i,9,5] = f29_2(tk[i]); H[i,1,6] = f31_2(tk[i]); H[i,2,6] = f32_2(tk[i]); H[i,3,6] = f33_2(tk[i]); H[i,4,6] = f34_2(tk[i]); H[i,5,6] = f35_2(tk[i]); H[i,6,6] = f36_2(tk[i]); H[i,7,6] = f37_2(tk[i]); H[i,8,6] = f38_2(tk[i]); H[i,9,6] = f39_2(tk[i]); end

Информационная матрица Фишера

На основании трехмерной матрицы Якоби с помощью двойной сумму формируем информационную матрицу Фишера. Информационная матрица Фишера играет ключевую роль в оценивании параметров траектории в задачах статистического моделирования, особенно когда речь идет о оценке параметров в динамических системах или процессах.

# расчет информационной матрицы Фишера for L = 1:9 # перебор координат матрицы Фишера for J = 1:9 # перебор координат матрицы Фишера G[L,J] = 0; # очистка суммы for i = 1:6 # перебор функций для каждого измерителя S0 = 0; # очистка суммы for j = 1:length(tk) S0 = S0+ H[j,J,i]*(1/D[i])*H[j,L,i]; # вторая сумма end G[L,J] = G[L,J]+S0; #первая сумма end end end

Корреляционная матрица

Корреляционная матрица вычисляется как обратная для матрицы Фишера.

# расчет корреляционной матрицы Ka = G^-1;

Функция для расчета параметров всех эллипсоидов

function ellipsoid5(x0,y0,z0,K) # радиусы эллипсоида a = 1; b = 1; c = 1; # изменения углов teta = 0:0.05:pi; fi = 0:0.05:(2*pi); x = zeros(1,length(teta)*length(fi)) y = zeros(1,length(teta)*length(fi)) z = zeros(1,length(teta)*length(fi)) x2 = zeros(1,length(teta)*length(fi)) y2 = zeros(1,length(teta)*length(fi)) z2 = zeros(1,length(teta)*length(fi)) k = 1; for i = 1:length(teta) for j = 1:length(fi) x[k] = a*sin(teta[i])*cos(fi[j]); y[k] = b*sin(teta[i])*sin(fi[j]); z[k] = c*cos(teta[i]); k = k+1; end end for i = 1:length(x) A = [x[i] y[i] z[i] 0 0 0 0 0 0]*K; x2[i] = A[1]; y2[i] = A[2]; z2[i] = A[3]; end X = vec(x2+x0*ones(1,length(teta)*length(fi))) Y = vec(y2+y0*ones(1,length(teta)*length(fi))) Z = vec(z2+z0*ones(1,length(teta)*length(fi))) return X, Y, Z end

# расчет матрицы ошибок траектории gr() K(t)= F(t)*Ka*(F(t)'); # построение зависимости от времени для матрицы ошибок for i = 1:length(tk) K_out[i,:, :] = K(tk[i]) end #figure эллипсоиды = plot3d([], [], [], legend=false) for i = 1:10:length(tk) Kx_f = [K_out[i,1,1] K_out[i,1,2] K_out[i,1,3] K_out[i,1,4] K_out[i,1,5] K_out[i,1,6] K_out[i,1,7] K_out[i,1,8] K_out[i,1,9]; K_out[i,2,1] K_out[i,2,2] K_out[i,2,3] K_out[i,2,4] K_out[i,2,5] K_out[i,2,6] K_out[i,2,7] K_out[i,2,8] K_out[i,2,9]; K_out[i,3,1] K_out[i,3,2] K_out[i,3,3] K_out[i,3,4] K_out[i,3,5] K_out[i,3,6] K_out[i,3,7] K_out[i,3,8] K_out[i,3,9]; K_out[i,4,1] K_out[i,4,2] K_out[i,4,3] K_out[i,4,4] K_out[i,4,5] K_out[i,4,6] K_out[i,4,7] K_out[i,4,8] K_out[i,4,9]; K_out[i,5,1] K_out[i,5,2] K_out[i,5,3] K_out[i,5,4] K_out[i,5,5] K_out[i,5,6] K_out[i,5,7] K_out[i,5,8] K_out[i,5,9]; K_out[i,6,1] K_out[i,6,2] K_out[i,6,3] K_out[i,6,4] K_out[i,6,5] K_out[i,6,6] K_out[i,6,7] K_out[i,6,8] K_out[i,6,9]; K_out[i,7,1] K_out[i,7,2] K_out[i,7,3] K_out[i,7,4] K_out[i,7,5] K_out[i,7,6] K_out[i,7,7] K_out[i,7,8] K_out[i,7,9]; K_out[i,8,1] K_out[i,8,2] K_out[i,8,3] K_out[i,8,4] K_out[i,8,5] K_out[i,8,6] K_out[i,8,7] K_out[i,8,8] K_out[i,8,9]; K_out[i,9,1] K_out[i,9,2] K_out[i,9,3] K_out[i,9,4] K_out[i,9,5] K_out[i,9,6] K_out[i,9,7] K_out[i,9,8] K_out[i,9,9]]; X, Y, Z = ellipsoid5(x(tk[i]),y(tk[i]),z(tk[i]),Kx_f) plot3d!(эллипсоиды, X, Y, Z) #hold on end scatter!(эллипсоиды, [coord1[1]], [coord1[2]], [coord1[3]], markershape=:dtriangle, markercolor=:red, markersize=2) scatter!(эллипсоиды, [coord2[1]], [coord2[2]], [coord2[3]], markershape=:dtriangle, markercolor=:red, markersize=2) display(эллипсоиды)

Дисперсии РЛС выбраны для наилучшего отображения эллипсоидов, для реальных локаторов они могут быть меньше и размер эллипсоидов будет меньше, также для более длинных участков траектории размер информационной матрицы Фишера будет больше. Соответственно значения корреляционной матрицы будет тем меньше чем большее количество точек траектории анализируется.