В данной статье будем реализовывать оптимальный фильтр Калмана с помощью среды моделирования Engee.
Структура навигационной системы будет представлять собой комбинацию бесплатформенной навигационной системы + спутниковой навигационной системы (СНС).
БИНС
Реальный режим работы БИНС отличается от режима, описываемого уравнениями идеальной работы, поскольку измерительные элементы (гироскопы и акселерометры), обладают собственными методическими и инструментальными погрешностями, начальные условия практически не могут быть введены абсолютно точно и, кроме того, существуют погрешности вычислителя, входящего в состав БИНС.
Под влиянием этих факторов БИНС функционирует в так называемом возмущенном режиме и выходная информация будет содержать погрешности, вызванные влиянием возмущений.
Основного уравнение инерциальной навигации в векторной форме записи имеет вид:
Где:
R - вектор положения объекта (геоцентрический радиус-вектор точки места ЛА),
g(R) - градиент поля тяготения
n - вектор кажущегося ускорения.
Накопление со временем ошибки определения параметров движения будем корректировать с помощью дополнительной системы. Существуют различные способы коррекции БИНС такие как доплеровский измеритель скоростей, астрокоррекция, коррекция от спутниковых навигационных систем. Коррекцию с помощью спутниковой навигационной системы реализовать наиболее просто так как достаточно электрически соединить необходимые датчики и создать алгоритмическое обеспечение для комплексирования.
СНС
Спутниковые навигационные системы позволяют определять координаты и скорости потребителя с точностью, недоступной для любого другого современного навигационного средства. Среднеквадратические погрешности составляют 10 - 12 м по каждой из координат и около 0,05 м/с по проекциям скорости. В дифференциальном режиме работы погрешности определения координат могут быть снижены до 1-2 м на удалении до нескольких десятков километров от контрольно-корректирующей станции (ККС).
Основными источниками погрешностей измерений спутниковой навигационной системы являются:
— качество навигационного сигнала (неточность прогноза эфемерид и уход бортового эталона времени (БЭВ));
— условия распространения радиоволн (задержки сигнала в ионосфере и в тропосфере; многолучевость распространения сигналов);
— несовершенство аппаратуры потребителя (шумы приемника; вычислительные погрешности; методические погрешности алгоритмов).
Ниже представлен набор скриптов с помощью которых реализовано комплексирование БИНС и спутниковой системы навигации
Инициализация пустых массивов
X_out=zeros(19,1); w = zeros(19,1); sigma = zeros(19,1); D = zeros(19,1); psi = zeros(19,1); Fi = zeros(36001,1); lam = zeros(36001,1); gam_list = zeros(36001,1); psi_list = zeros(36001,1); Tr_list = zeros(36001,1); ppm_dist_list = zeros(36001,1); V_e = zeros(36001,1); V_n = zeros(36001,1); turn_start = zeros(19,1); turn_end = zeros(19,1); X_out_list = zeros(19,36001); X_full_list = zeros(19,36001); X_k_list = zeros(19,36001); X_m_list = zeros(19,36001); X_m = zeros(19,1); X = zeros(1,4); Y = zeros(1,4); Z = zeros(1,4); Xa = zeros(36001,1); Ya = zeros(36001,1); Za = zeros(36001,1); Xk2 = zeros(36001,1); Yk2 = zeros(36001,1); Zk2 = zeros(36001,1); dY = zeros(36001,1); dX = zeros(36001,1); Pk_list = zeros(4,36001); Pk_o = zeros(19,19);
Векторы содержащие долготу и широту в поворотных точках маршрута
Полет будет проходить на постоянной высоте, поворотные точки заданы в виде массивов с широтой и долготой.
fi_o = [55, 56.58, 56.46, 56.96];#широта lam_o = [37, 37.5, 38, 38.5];#долгота H_o = 1000; #высота не меняется поэтому запишем как: H = 1000;
Константы необходимые для расчета
#using LinearAlgebra # начальные зачения переменных lur_dist = 0 UR = 0 Tr = 0 V_H=0; tetta=0; Rz=6371000 ## радиус земли t=3600 ## время расчета dt = 0.1; #шаг времени gam = pi*46/180; #предельный крен ЛА g=9.78049; #ускорение свободного падения u=(pi*15/3600)/180; w0=sqrt(g/Rz);#частота шулера a=6378245;# большая полуось b=6356863; #малая полуось e=sqrt(a^2-b^2)/a; # эксцентриситет q=0.00346775; # отношение центробежной силы, возникающей вследствие вращения Земли, к силе тяжести на экваторе bet=0.0053171; bet1=71*10^-7; # счетчики i=1;#координатный счетчик k=1;#счетчик разворотов j=1;#прочее ## перевод из градусов в радианы fi_o=fi_o.*pi/180 lam_o=lam_o.*pi/180
Начальная выставка БИНС
На этапе начальной выставки:
Определение начальных значений скорости и координат местоположения объекта. Например, для БИНС, установленной на беспилотном летательном аппарате (БПЛА), начальные значения скорости и координат известны априорно и равны значениям, определённым пилотажно-навигационными устройствами на борту самолёта-носителя.
Определение ориентации измерительных осей акселерометров. Вычисляется начальное значение матрицы направляющих косинусов, которая характеризует взаимную ориентацию координатного трёхгранника, связанного с блоком измерительных элементов БИНС, и системы координат, принятой за базовую.
lam_error=6/Rz;# lam error fi_error=6/Rz;# fi error V_e_error=0.05; #V_e error V_n_error=0.05;#V_n error psi_error=0.25*pi/180;#курс gamma_error=0.03*pi/180;#крен tetta_error=0.03*pi/180;#тангаж
Расчет курса для каждой поворотной точки маршрута
Ортодромия — кратчайшее расстояние между двумя точками на поверхности Земли (сферы)
for j = 2:1:4 w[j-1]=lam_o[j]-lam_o[j-1];#изменение долготы sigma[j-1]=acos(sin(fi_o[j-1])*sin(fi_o[j])+cos(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1]));#длина ортодромии в градусах между ППМ(j-1) и ППМ(j) D[j-1]=Rz*sigma[j-1]; #пересчет из градусов в метры длины ортодромии #угол курса(обратная геодезическая задача на сфере) psi[j-1]=atan(cos(fi_o[j])*sin(w[j-1]),cos(fi_o[j-1])*sin(fi_o[j])-sin(fi_o[j-1])*cos(fi_o[j])*cos(w[j-1])) #цикл проверки угола курса = [0:2*pi] if (psi[j-1] <= 0) psi[j-1] = psi[j-1] + 2*pi; end if (psi[j-1] > 2*pi) psi[j-1] = psi[j-1] - 2*pi; end end
Расчет пройденного пути и средней скорости движения
Пройденный пусть вычисляется как сумма расстояний между всеми поворотными точками маршрута, средняя скорость вычисляется как весь пройденный путь / время моделирования
sum_distance = sum(D);#суммируем расстояния от ппмов dist=sum_distance; V = sum_distance/t;#скорость движения (без ускорения) #задаем стартовые значения Fi[1] = fi_o[1] lam[1] = lam_o[1]; gam_list[i]=0; psi_list[i]=-psi[1]+2*pi;
Расчет параметров траектории в точках между ППМ
for k = 1:3 if k!=1 w[k]=lam_o[k+1]-lam[i];#изменение долготы sigma[k]=acos(sin(Fi[i])*sin(fi_o[k+1])+cos(Fi[i])*cos(fi_o[k+1])*cos(w[k]));#длина ортодромии в градусах между ППМ(k) и ППМ(k+1) D[k]=Rz*sigma[k] #%пересчет из градусов в метры длины ортодромии #угол курса(обратная геодезическая задача на сфере) psi[k]=atan(cos(fi_o[k+1])*sin(w[k]),cos(Fi[i])*sin(fi_o[k+1])-sin(Fi[i])*cos(fi_o[k+1])*cos(w[k])) #цикл проверки угола курса = [0:2pi] if (psi[k] <= 0) psi[k] = psi[k] + 2*pi; end if (psi[k] > 2*pi) psi[k] = psi[k] - 2*pi; end sum_distance = sum(D);#суммируем расстояния от ппмов V = sum_distance/t; #скорость движения (без ускорения) end #Проекции абсолютной скорости #вертикальная составляющая отсутствует #горизонатальные составляющие Ve = V*sin(psi[k]);#восток Vn = V*cos(psi[k]);#север #угловые скорости w_E = Vn/Rz; w_N = Ve/Rz; if (k != 3) UR = psi[k+1] - psi[k]; if UR<0 gam=-gam; end Rr = V^2 / (9.81 * tan(gam));#радиус разворота Tr = Rr*UR/V; #время разворота lur_dist = Rr*tan(0.5*UR); #расстояние ЛУР Tr_list[k]=Tr; end ppm_dist = Rz*sigma[k]; #длина пути от ппм(i-1) до ппм(i) ppm_dist_past = ppm_dist+1; #построение траектории пока угол курса остается постоянным while (lur_dist < ppm_dist)&&(ppm_dist_past > ppm_dist) i = i + 1; gam_list[i]=0; ppm_dist_past = ppm_dist; #приращение координат dfi = w_E; dlam = w_N/cos(Fi[i-1]); #нахождение положения через dt Fi[i] = Fi[i-1] + dfi*dt; lam[i] = lam[i-1] + dlam*dt; #Пересчет длины ортодромии c учетом пройденного расстояния sig = acos(sin(Fi[i])*sin(fi_o[k+1])+cos(Fi[i])*cos(fi_o[k+1])*cos(lam_o[k+1] - lam[i])); ppm_dist = Rz*sig; ppm_dist_list[i] = ppm_dist; psi_list[i]=-psi[k]+2*pi; end dPsi = dt*UR/Tr;#приращение угла курса за dt во время ЛУР #построение траектории при изменении угла курса (ЛУР) turn_start[k]=i; if (k != 3) for j = 0:dt:Tr i = i + 1; gam_list[i]=gam; psi[k] = psi[k] + dPsi;#%изменение угла курса за время dt Ve = V*sin(psi[k]); Vn = V*cos(psi[k]); w_E = Vn/Rz; w_N = Ve/Rz; dfi = w_E; dlam = w_N/cos(Fi[i-1]); Fi[i] = Fi[i-1] + dfi*dt; lam[i] = lam[i-1] + dlam*dt; psi_list[i]=-psi[k]+2*pi; end end turn_end[k]=i; end i=1;
Алгоритм оптимального фильтра Калмана
В исследовании будет рассмотрена БИНС с позиционной и скоростной коррекцией от спутниковой системы. Траекторные условия: полет по локсодромии на постоянной высоте с постоянной скоростью.
Динамическая модель в виде системы дифференциальных уравнений:
Где:
X – вектор состояния системы,
F – матрица динамики системы;
G – матрица шумов системы;
W - вектор шумов системы;
Z – вектор измерений;
H – матрица связи;
V – вектор шумов измерений.
Для моделирования комплексной обработки измерений удобно использовать дискретную форму оптимального фильтра Калмана:
Где:
Xk – оценка вектора состояния на k-м такте,
Ф – переходная матрица системы,
Г – матрица случайных воздействий,
Sk, Pk - априорная и апостериорная матрицы ковариации на k-ом такте,
Q1,R1 - симметрические неотрицательно определенная и положительно определенная матрицы интенсивностей шумов дискретной системы.
Реализация фильтра Калмана в виде скрипта Engee
for i=1:1:length(lam)-1 i=i+1; gamma=gam_list[i-1]; # показания гироскопов (ошибки) # ось 1 x_gyro_zero_error=0.001*pi/180; x_gyro_noise=x_gyro_zero_error*0.01*randn(1); x_gyro_koef_error=2*10^-5; # ось 2 y_gyro_zero_error=0.001*pi/180; y_gyro_noise=y_gyro_zero_error*0.01*randn(1); y_gyro_koef_error=2*10^-5; # ось 3 z_gyro_zero_error=0.001*pi/180; z_gyro_noise=z_gyro_zero_error*0.01*randn(1); z_gyro_koef_error=2*10^-5; # показания акселерометров (ошибки) # ось 1 x_ac_zero_error=g*60*(10^-6); x_ac_noise=x_ac_zero_error*0.01*randn(1); x_ac_koef_error=3*10^-6; # ось 2 y_ac_zero_error=g*60*(10^-6); y_ac_noise=y_ac_zero_error*0.01*randn(1); y_ac_koef_error=3*10^-6; # ось 3 z_ac_zero_error=g*60*(10^-6); z_ac_noise=z_ac_zero_error*0.01*randn(1); z_ac_koef_error=3*10^-6; # скорости V_e[i-1]=V*cos(psi_list[i-1]); V_n[i-1]=V*sin(psi_list[i-1]); if i==2 d_V_e=0; d_V_n=0; else d_V_e=(V_e[i-1]-V_e[i-2]); d_V_n=(V_n[i-1]-V_n[i-2]); end #проекции угловой скорости вращения Земли u_n=u*cos(Fi[i-1]); u_H=u*sin(Fi[i-1]); #радиусы кривизны, соответственно, меридионального и перпендикулярного ему сечений ro1=(a*(1-e^2)/(sqrt(1-e^2*sin(Fi[i-1])^2)^3))+H; ro2=(a/sqrt(1-e^2*sin(Fi[i-1])^2))+H; #относительный угловые скорости omega_e_1=-V_n[i-1]/ro1; omega_n_1=V_e[i-1]/ro2; omega_H_1=tan(Fi[i-1])*(V_e[i-1]/ro2); #абсолютные угловые скорости omega_e=omega_e_1; omega_n=omega_n_1+u_n; omega_H=omega_H_1+u_H; # ошибки определения координат бинс if i==2 x1=6; x2=6; else x1=lam_error*ro2*cos(Fi[i-1]); x2=fi_error*ro1; end # ошибки в определении скорости изменения координат БИНC %% if i==2 x3=0.05; x4=0.05; else x3=V_e_error+(V_H/ro2+omega_e*tan(Fi[i-1]))*x1+omega_H*x2; x4=V_n_error+x1*V_H/ro1; end # ошибки ориентации вычисленного БИНС трехгранника относительно базового turn=[sin(psi_list[i-1])*tan(tetta) cos(psi_list[i-1])*tan(tetta) -1; cos(psi_list[i-1]) -sin(psi_list[i-1]) 0; sin(psi_list[i-1])/cos(tetta) cos(psi_list[i-1])/cos(tetta) 0]; eler_er=[psi_error;tetta_error;gamma_error]; or_er=turn^-1*eler_er; alfa=or_er[1]+X_out[5]; betta=or_er[2]+X_out[6]; gamma=or_er[3]+X_out[7]; alfa1=-x2/ro1; betta1=x1/ro2; gamma1=x1*tan(Fi[i-1])/ro2; alfa2=alfa+alfa1; betta2=betta+betta1; gamma2=gamma+gamma1; # проекции силы тяжести в нормальной ск g0=g*(1+bet*sin(Fi[i-1])^2+bet1*sin(2*Fi[i-1])^2); g_y=g0*sin(2*Fi[i-1])*H*((e^2)/a-2*q)/a; g_z=g0+H*((3*H/a)-2*q*g*cos(Fi[i-1])^2+e^2*(3*sin(Fi[i-1])^2-1)-q*(1+6*sin(Fi[i-1])^2))/a; # кажущиеся ускорения в нормальной ск n_e=d_V_e-(omega_H_1+2*u_H)*V_n[i-1]+(omega_n_1+2*u_n)*V_H; n_n=d_V_n+(omega_H_1+2*u_H)*V_e[i-1]-omega_e_1*V_H-g_y; n_H=-(omega_n_1+2*u_n)*V_e[i-1]+omega_e_1*V_n[i-1]-g_z; # CCCCCCCCCCCCC C=[-sin(psi_list[i-1])*cos(tetta) sin(psi_list[i-1])*sin(tetta)*cos(gamma)+cos(psi_list[i-1])*sin(gamma) -sin(psi_list[i-1])*sin(tetta)*sin(gamma)+cos(psi_list[i-1])*cos(gamma); cos(psi_list[i-1])*cos(tetta) -cos(psi_list[i-1])*sin(tetta)*cos(gamma)+sin(psi_list[i-1])*sin(gamma) cos(psi_list[i-1])*sin(tetta)*cos(gamma)+sin(psi_list[i-1])*cos(gamma); sin(tetta) cos(tetta)*cos(gamma) -cos(tetta)*sin(gamma)]; # пересчет из нормальной в связанную ск omega_id=[omega_e;omega_n;omega_H]; omega_c=C^-1*omega_id; omega_x=omega_c[1]; omega_y=omega_c[2]; omega_z=omega_c[3]; n_id=[n_e;n_n;n_H]; n_c=C^-1*n_id; n_x=n_c[1]; n_y=n_c[2]; n_z=n_c[3]; # проекция скорости изменения абсолютной угловой скорости вращения на вертикаль места dot_omega_H=(u*cos(Fi[i-1])*V_n[i-1]+n_e*tan(Fi[i-1])+V_H*omega_H+V_e[i-1]*V_n[i-1]/(ro2*cos(Fi[i-1])^2))/ro2; # матрица динамики системы матрица динамики системы полная F_full=zeros(19,19); F_full[1,3]=1; F_full[2,4]=1; F_full[3,:]=[-w0^2+omega_n^2+omega_H^2,dot_omega_H-omega_e*omega_n,0,2*omega_H,0,n_H,0,0,0,0,0,0,0,C[1,1],C[1,2],C[1,3],n_x*C[1,1],n_y*C[1,2],n_z*C[1,3]]; F_full[4,:]=[-(dot_omega_H-omega_e*omega_n),-w0^2+omega_e^2+omega_H^2,-2*omega_H,0,-n_H,0,n_e,0,0,0,0,0,0,C[2,1],C[2,2],C[2,3],n_x*C[2,1],n_y*C[2,2],n_z*C[2,3]]; F_full[5,:]=[0,0,0,0,0,omega_H,-omega_n,C[1,1],C[1,2],C[1,3],omega_x*C[1,1],omega_y*C[1,2],omega_z*C[1,3],0,0,0,0,0,0]; F_full[6,:]=[0,0,0,0,-omega_H,0,omega_e,C[2,1],C[2,2],C[2,3],omega_x*C[2,1],omega_y*C[2,2],omega_z*C[2,3],0,0,0,0,0,0]; F_full[7,:]=[0,0,0,0,omega_H,-omega_e,0,C[3,1],C[3,2],C[3,3],omega_x*C[3,1],omega_y*C[3,2],omega_z*C[3,3],0,0,0,0,0,0]; # матрица шумов системы матрица шумов системы G=zeros(19,6); G[3,:]=[0,0,0,C[1,1],C[1,2],C[1,3]]; G[4,:]=[0,0,0,C[2,1],C[2,2],C[2,3]]; G[5,:]=[C[1,1],C[1,2],C[1,3],0,0,0]; G[6,:]=[C[2,1],C[2,2],C[2,3],0,0,0]; G[7,:]=[C[3,1],C[3,2],C[3,3],0,0,0]; # вектор параметров для оценки вектора X_full=[x1; x2; x3; x4; alfa2; betta2; gamma2; x_gyro_zero_error; y_gyro_zero_error; z_gyro_zero_error; x_gyro_koef_error; y_gyro_koef_error; z_gyro_koef_error; x_ac_zero_error; y_ac_zero_error; z_ac_zero_error; x_ac_koef_error; y_ac_koef_error; z_ac_koef_error]; # вектор шумов измерений W=[x_gyro_noise; y_gyro_noise; z_gyro_noise; x_ac_noise; y_ac_noise; z_ac_noise]; # формирование вектора оцениваемых параметров X_out=(F_full*X_full+G*W)*dt; X_out_list[:,i-1]=X_out; X_full_list[:,i-1]=X_full; # широта и долгота накопление ошибки lam_error=lam_error+X_out[1]/(ro2*cos(Fi[i-1]));# lam error fi_error=fi_error+X_out[2]/ro1;# # проекции вектора скорости fi error V_e_error=V_e_error+X_out[3]; # V_e error V_n_error=V_n_error+X_out[4];# V_n error Xreal=[lam[i-1];Fi[i-1];V_e[i-1];V_n[i-1]]; # матрица измерений Hk=zeros(4,19);# матрица связи вектора сост и вектора измерений Hk[1,1]=1/(ro2*cos(Fi[i-1])); Hk[2,2]=1/ro1; Hk[3,:]=[-(V_H/ro2+omega_e*tan(Fi[i-1])),-omega_H,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; Hk[4,:]=[-V_H/ro1,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]; # случайный шум для координат noise_lam=5*randn(1)/ro2*cos(Fi[i-1]); noise_fi=5*randn(1)/ro1; # случайный шум для скоростей noise_V_e=0.05*randn(1); noise_V_n=0.05*randn(1); # матрица шумов системы VV=[noise_lam;noise_fi;noise_V_e;noise_V_n];# вектор шумов измерений снс Zsns=Xreal+VV;# параметры формируемые приёмником СНС показание снс Zins=Xreal+Hk*X_full;# параметры формируемые инерциальной системой навигации показание инс Zk=Zins-Zsns; ## вектор измерений #матрицы интенсивности шумов измерений Rk=[5/(ro2*cos(Fi[i-1])) 0 0 0; 0 5/ro1 0 0; 0 0 0.05 0; 0 0 0 0.05]; Rk=Rk.^2; Rk=Rk/dt; Qk=[x_gyro_zero_error*0.01 0 0 0 0 0; 0 y_gyro_zero_error*0.01 0 0 0 0; 0 0 z_gyro_zero_error*0.01 0 0 0; 0 0 0 x_ac_zero_error*0.01 0 0; 0 0 0 0 y_ac_zero_error*0.01 0; 0 0 0 0 0 z_ac_zero_error*0.01]; Qk=Qk.^2; Qk=Qk/dt; if i==2 ## ковариационная матрица (апостериорная) Pk_o=[x1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 x2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 x3 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 x4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 alfa2 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 betta2 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 gamma2 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 x_gyro_zero_error 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 y_gyro_zero_error 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 z_gyro_zero_error 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 x_gyro_koef_error 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 y_gyro_koef_error 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 z_gyro_koef_error 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 x_ac_zero_error 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 y_ac_zero_error 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 z_ac_zero_error 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 x_ac_koef_error 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 y_ac_koef_error 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 z_ac_koef_error]; Pk_o=Pk_o.^2; end # накопление матрицы динамики Fk=(eye(19)+F_full*dt+((F_full*dt)^2)/2); # накопление матрицы интенсивности шумов измерений Gk=(eye(19)+F_full*dt/2+((F_full*dt)^2)/6)*G*dt; # расчет априорной матрицы ковариаций Sk=Fk*Pk_o*Fk'+Gk*Qk*Gk'; K=Sk*Hk'*(Hk*Sk*Hk'+Rk)^-1; Pk_o=(eye(19)-K*Hk)*Sk; # пересчет апостериорной матрицы ковариаций X_m=Fk*X_m+K*(Zk-Hk*Fk*X_m); # пересчет вектора оценки состояния системы X_k=X_full-X_m; ## запись каждой итерации X_k_list[:,i-1]=X_k; X_m_list[:,i-1]=X_m; ## расчет СКО для каждой итерации Pk_list[1,i-1]=sqrt(Pk_o[1,1]); Pk_list[2,i-1]=sqrt(Pk_o[2,2]); Pk_list[3,i-1]=sqrt(Pk_o[3,3]); Pk_list[4,i-1]=sqrt(Pk_o[4,4]); end #перевод в декартову систему координат for i=1:1:length(lam_o) X[i]=(Rz+H_o)*sin(fi_o[i]-fi_o[1])*cos(lam_o[i]-lam_o[1]); Y[i]=(Rz+H_o)*sin(fi_o[i]-fi_o[1])*sin(lam_o[i]-lam_o[1]); Z[i]=(Rz+H_o)*sin(fi_o[i]-fi_o[1]); end
Пересчет параметров траектории к плоскости горизонта
for i=1:1:length(lam)-1 Xa[i]=(Rz+H_o)*sin(Fi[i]-Fi[1])*cos(lam[i]-lam[1]); Ya[i]=(Rz+H_o)*sin(Fi[i]-Fi[1])*sin(lam[i]-lam[1]); Za[i]=(Rz+H_o)*sin(Fi[i]-Fi[1]); Xk2[i]=(Rz+H_o)*sin(Fi[i]-Fi[1])*cos(lam[i]-lam[1])+X_k_list[1,i]; Yk2[i]=(Rz+H_o)*sin(Fi[i]-Fi[1])*sin(lam[i]-lam[1])+X_k_list[2,i]; Zk2[i]=(Rz+H_o)*sin(Fi[i]-Fi[1]); dY[i]=Ya[i]-Yk2[i]; dX[i]=Xa[i]-Xk2[i]; end
Построение на графиках результатов работы фильтра
plot(ppm_dist_list,title="Расстояние до следующего ППМ") ##
Ya[end] = Ya[end-1] Xa[end] = Xa[end-1] plot(Ya,Xa,title="Траектория движения") ##
plot(dY,title="Оценка ошибки по координате Y")
plot(dX,title="Оценка ошибки по координате X")
plot(X_full_list[4,:],title="Ошибка оценки для скорости X") ##
plot(X_k_list[4,:],title="Ошибка оценки для скорости Y") ## построение графика
plot(Pk_list[1,:],title="Среднеквадратичная оценка ошибки для координаты X") ## среднеквадратичная оценка
plot(Pk_list[2,:],title="Среднеквадратичная оценка ошибки для координаты Y") ## среднеквадратичная оценка
plot(Pk_list[3,:],title="Среднеквадратичная оценка ошибки для скорости Х") ## среднеквадратичная оценка
plot(Pk_list[4,:],title="Среднеквадратичная оценка ошибки для скорости Y") ## среднеквадратичная оценка
В ходе написания данной статьи разработан алгоритм комплексной обработки информации для выбранной компоновки навигационной системы (СНС+БИНС), проведено моделирование в среде Engee. Проведен анализ точности, обеспечиваемой алгоритмами КОИ. Графики СКО ошибки оценки сходятся, что свидетельствует о правильной работе фильтра.
