Постановка задачи
Для того чтобы определить вероятные положения летательного аппарата в окрестностях траектории необходимо использовать комплексную обработку данных полученных с различных источников, в рамках данной статьи предполагается что в основу расчета берем усредненные параметры участка траектории ЛА, известные координаты РЛС которые определяют его положение, дисперсии для каждой РЛС (в рамках данного моделирования берем две, но в произвольном случае может быть любое количество)
Подобные расчеты требуются для того чтобы определить как близко могут пролететь самолеты один относительно другого в сложных навигационных условиях (например в условиях заглушенного сигнала 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(эллипсоиды)
Дисперсии РЛС выбраны для наилучшего отображения эллипсоидов, для реальных локаторов они могут быть меньше и размер эллипсоидов будет меньше, также для более длинных участков траектории размер информационной матрицы Фишера будет больше. Соответственно значения корреляционной матрицы будет тем меньше чем большее количество точек траектории анализируется.