Использование фильтра Калмана для построения траектории ЛА
goltsevИспользование оптимального фильтра Калмана для оценки параметров ЛА
В данной статье будем реализовывать оптимальный фильтр Калмана с помощью среды моделирования Engee.
Структура навигационной системы будет представлять собой комбинацию бесплатформенной навигационной системы + спутниковой навигационной системы (СНС).
Рисунок 1 - структурная схема навигационного комплекса
БИНС
Реальный режим работы БИНС отличается от режима, описываемого уравнениями идеальной работы, поскольку измерительные элементы (гироскопы и акселерометры), обладают собственными методическими и инструментальными погрешностями, начальные условия практически не могут быть введены абсолютно точно и, кроме того, существуют погрешности вычислителя, входящего в состав БИНС.
Под влиянием этих факторов БИНС функционирует в так называемом возмущенном режиме и выходная информация будет содержать погрешности, вызванные влиянием возмущений.
Основного уравнение инерциальной навигации в векторной форме записи имеет вид:
Где:
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;
.png)
Рисунок 2 - точки маршрута на карте
Константы необходимые для расчета
#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;#тангаж
Расчет курса для каждой поворотной точки маршрута
Ортодромия — кратчайшее расстояние между двумя точками на поверхности Земли (сферы)
.png)
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;
Код оптимального фильтра Калмана
В исследовании будет рассмотрена БИНС с позиционной и скоростной коррекцией от спутниковой системы. Траекторные условия: полет по локсодромии на постоянной высоте с постоянной скоростью.
Динамическая модель в виде системы дифференциальных уравнений:
.png)
Где:
X – вектор состояния системы,
F – матрица динамики системы;
G – матрица шумов системы;
W - вектор шумов системы;
Z – вектор измерений;
H – матрица связи;
V – вектор шумов измерений.
Для моделирования комплексной обработки измерений удобно использовать дискретную форму оптимального фильтра Калмана:
.png)
Где:
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. Проведен анализ точности, обеспечиваемой алгоритмами КОИ. Графики СКО ошибки оценки сходятся, что свидетельствует о правильной работе фильтра.