МИНИСТЕРСТВО ОБРАЗОВАНИЯ И НАУКИ РОССИЙСКОЙ ФЕДЕРАЦИИ

ФЕДЕРАЛЬНОЕ ГОСУДАРСТВЕННОЕ БЮДЖЕТНОЕ ОБРАЗОВАТЕЛЬНОЕ

УЧРЕЖДЕНИЕ ВЫСШЕГО ОБРАЗОВАНИЯ
« МОСКОВСКИЙ АВИАЦИОННЫЙ ИНСТИТУТ
(НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ)» (МАИ)
Кафедра № 301

ЛАБОРАТОРНАЯ РАБОТА №1
“система управления продольным движением летательного
аппарата с использованием фильтра Калмана.”

Студентов : Нгуен Вьет Ань

Пхон Мьинт Мо
Группа: М3О-104Мки-21

Руководитель: Михайлин Д. А

Москва
2021
Задание
Построить оптимальный фильтр Калмана для продольного движения самолета.

Уравнения движения
Движение БПЛА в продольной плоскости описывается следующей системой уравнений:

Δ Ḣ =ΔV y

{
π V0 δ
V̇ y = ∗( Ý αa∗( Δα+ Δα ω ) + Ý a ∗Δδ в ) ,
в

180
Δ α̇= Δ ω z− Ý αa∗( Δα + Δα ω ) −Ý δa ∗Δ δ в ,
в

Δ ω̇ z = Ḿ αz ∗( Δα +α ω ) + Ḿ ωz ∗Δ ωz + Ḿ δz ∗Δ δ в ,
z в

Δ ϑ̇=Δ ω z ,
−1 180 180
Δ α̇ ω= ∗Δα ω + ∗Δα + ∗η ,
T πT V 0 πT V 0 α
Δ̇ α =0

В уравнениях, описывающих продольное движение ЛА, были использованы величины:

 H – высота самолёта;
 V – скорость;
 α – угол атаки;
 ϑ – угол тангажа;
 ω z – угловая скорость тангажа;
 δ в – руль высоты;
 ηα – белый шум;
 Δ α ω – приращение угла скорости вертикального ветра;
 Δ α – приращение систематической составляющей скорости вертикального ветра;
α
 Ý a – производная приведённой к произведению массы БПЛА на скорость
установившегося полёта аэродинамической подъёмной силы крыла по α;
δ
 Ý a – производная приведённой к произведению массы БПЛА
в

на скорость установившегося полёта аэродинамической

подъёмной силы по δ в;
α
 Ḿ z – производная приведённого к моменту инерции относительно связанной оси
OZ момента ϑ по α;
δ
 Ḿ z – производная приведённого к моменту инерции относительно связанной оси
в

OZ момента ϑ по δ в;
ω
 Ḿ z – приведённый к моменту инерции относительно связанной оси OZ
z

демпфирующий момент ϑ .

Управление по рулю высоты

U =3∗( ϑ−ϑ зад. ) +1.5∗ω z

2
Алгоритм оптимального фильтра Калмана

1) Sk =F k ∗Pk−1∗F Tk +Gk ∗Q1∗GTk

2) K k =S k∗H T ∗(H∗S k∗H T + R1 )−1
3) ^x k =F k∗ x^ k−1 + Bk∗U k−1+ K k∗( Z k −H∗( F k∗ ^x k−1∗Bk ∗U k−1 ) )
4) Pk = ( E−K∗C )∗S k ,

где G - матрица эффективности шумов, P - матрица ковариаций, F - динамическая

матрица объекта, B – матрица эффективности управления, R - матрица шумов в канале
Q R
измерений, H - Матрица измерений, ^x k - оценка. и Q 1= ; R1 = .
Δt Δt

Рис. 1. Схема фильтра Калмана.

Начальные условия

100 м/с M δz в
-4
α
Ya
0,6 M ωz z
-0.7
δв
Ya
0,07 T 0,2 с.
M α
z
-3 Δt 0,01с.
ση α
1 м/с
3
 σVy
1 м/с σω z
0,0029
σα 0,1° σϑ 0,1

Начальная матрица ковариаций (дисперсии ошибок оценивания) P:

100 0 0 0 0 0 0
0 20 0 0 0 0 0
0 0 25 0 0 0 0
0 0 0 25 0 0 0
0 0 0 0 25 0 0
0 0 0 0 0 25 0
0 0 0 0 0 0 5

Матрица измерений H:

1 0 0 0 0 0 0
0 1 0 0 0 0 0
0 0 1 0 0 0 0
0 0 0 1 0 0 0
0 0 0 0 1 0 0

Матрица шумов в канале измерений R:

1 0 0 0 0
0 1 0 0 0
0 0 0,010 0 0
0 0 0 0,000841 0
0 0 0 0 0,010

Динамическая матрица объекта A:

0 1 0 0 0 0 0
0 0 1,047 0 0 1,047 0
0 0 -0,60 1 0 -0,60 0
0 0 -3 -0,70 0 -3 0
0 0 0 1 0 0 0
0 0 0 0 0 -5 2,86
0 0 0 0 0 0 0

Матрица эффективности управления B:

0
-0,0007
-0,0007
-0,0400
0
0
0

Матрица эффективности шумов G:

0
0
4
0
0
0
0,0286
0
Расчёты в MATLAB
clc, clear;
% Начальные условия
tn = 10; % время полета, сек.
d_t = 0.01; % сек.
t = 0 : d_t : tn;
n = length(t);
Vo = 100; % начальная скорость, м/с
T = 0.2; % постоянная времени формирующего фильтра, сек.
% Y_alfa_a - производная, приведённой к произведению массы БПЛА
% на скорость установившегося полёта аэродинамической
% подъёмной силы крыла по α
Y_alfa_a = 0.6;
% Y_delta_b - производная приведённой к произведению массы БПЛА
% на скорость установившегося полёта аэродинамической
% подъёмной силы по δв
Y_delta_b = 0.07;
% M_alfa_z - производная приведённого к моменту инерции относительно
% связанной оси OZ момента θ по α
M_alfa_z = -3;
% M_omega_z_a - приведённый к моменту инерции относительно
% связанной оси OZ демпфирующий момент θ
M_omega_z_a = -0.7;
% M_delta_b - производная приведённого к моменту инерции
% относительно связанной оси OZ момента θ по δв
M_delta_b = -4;
% Погрешности измерительных датчиков
sigma_eta_alfa = 0.1/sqrt(d_t);
sigma_V_y = 0.1/sqrt(d_t);
sigma_alfa = 0.01/sqrt(d_t);
sigma_omega_z = 0.0029/sqrt(d_t);
sigma_teta = 0.01/sqrt(d_t);
sigma_h = 0.1/sqrt(d_t);
% Корреляционная матрица шумов на входе
Q = [sigma_eta_alfa^2];
% ----------------------------------------------------------------------
% Матрицы
% G - матрица эффективности шумов
% P - матрица ковариаций
% A - динамическая матрица объекта
% B – матрица эффективности управления
% R - матрица шумов в канале измерений
% H - Матрица измерений
% ----------------------------------------------------------------------
E = eye(7);
% Матрица эффективности шумов
G = [0 0 0 0 0 (180 / (pi * T * Vo)) 0]';

5
% Матрица ковариаций
P = [100 0 0 0 0 0 0;
0 20 0 0 0 0 0;
0 0 25 0 0 0 0;
0 0 0 25 0 0 0;
0 0 0 0 25 0 0;
0 0 0 0 0 25 0;
0 0 0 0 0 0 5];
% Динамическая матрица объекта
A = [0 1 0 0 0 0 0;
0 0 (pi * Vo / 180) * Y_alfa_a 0 0 (pi * Vo / 180) * Y_alfa_a 0;
0 0 -Y_alfa_a 1 0 -Y_alfa_a 0;
0 0 M_alfa_z M_omega_z_a 0 M_alfa_z 0;
0 0 0 1 0 0 0;
0 0 0 0 0 -1/T (180 / (pi * T * Vo));
0 0 0 0 0 0 0];
% Матрица эффективности управления
B = [0 -Y_delta_b -Y_delta_b M_delta_b 0 0 0]';
% Матрица шумов в канале измерений
R = [sigma_h^2 0 0 0 0;
0 sigma_V_y^2 0 0 0;
0 0 sigma_alfa^2 0 0;
0 0 0 sigma_omega_z^2 0;
0 0 0 0 sigma_teta^2];
% Матрица измерений
H = [1 0 0 0 0 0 0;
0 1 0 0 0 0 0;
0 0 1 0 0 0 0;
0 0 0 1 0 0 0;
0 0 0 0 1 0 0];
% Оценённый вектор состояния(выход ОФК)
x = [0 0 0 0 0 0 5]';
% Проверка на наблюдаемость по Калману
M = [H' A'*H' A'^2*H' A'^3*H' A'^4*H' A'^5*H' A'^6*H'];
r = rank (M);
% Дискретный вид матриц
F = d_t*A + E; % дискретный вид динамической матрицы объекта
G = G *d_t; % дискретный вид матрицы эффективности шумов
B = B*d_t; % дискретный вид матрицы эффективности органов управления
% ----------------------------------------------------------------------
% Шумы в канале измерений
% ----------------------------------------------------------------------
noise_h = sigma_h*randn(n,1);
noise_V_y = sigma_V_y*randn(n,1);
noise_alfa = sigma_alfa*randn(n,1);
noise_omega_z = sigma_omega_z*randn(n,1);
noise_teta = sigma_teta*randn(n,1);
% шум на входе объекта (ветровой)
noises = [noise_h noise_V_y noise_alfa noise_omega_z noise_teta]';
noise_wind = sigma_eta_alfa * randn(n, 1);
% ----------------------------------------------------------------------

% Построение

6
% ----------------------------------------------------------------------
x_o=x;
for i =1:n
% Управление
u = 3*(x(5)-20)+1.5*x(4);
% Вычисление вектора состояния на следующем шаге
x = F*x+B*u+G*noise_wind(i);
% Уравнение измерений
Z = H*x+noises(:,i);
% Априорная матрица ковариаций (дисперсий ошибок оценивания)
P = F*P*F'+G*Q*G';
K = P*H'/(H*P*H'+R);
% Вычисление оценки
x_o = F*x_o + B*u + K*(Z - H*(F*x_o+B*u));
% Апостериорная матрица ковариаций
P=(E-K*H)*P;
d(:,i) = x; % запись вектора состояния
f(:,i) = x_o; % запись вектора оценок
for j=1:7
q(j,i) = P(j,j); % запись матрицы дисперсий ошибок оценивания
end;
end
% ----------------------------------------------------------------------
% Графики по компонентам вектора состояния
% ----------------------------------------------------------------------
titles = cell(14, 3);
titles{1, 1} = 'H(t), H*(t)'; titles{1, 2} = 't'; titles{1, 3} = 'H, H*';
titles{2, 1} = 'V(t), V*(t)'; titles{2, 2} = 't'; titles{2, 3} = 'V, V*';
titles{3, 1} = 'a(t), a*(t)'; titles{3, 2} = 't'; titles{3, 3} = 'a, a*';
titles{4, 1} = 'w_z(t), w_z*(t)'; titles{4, 2} = 't'; titles{4, 3} = 'w_z, w_z*';
titles{5, 1} = 'v(t), v*(t)'; titles{5, 2} = 't'; titles{5, 3} = 'v, v*';
titles{6, 1} = 'a_w(t), a_w*(t)'; titles{6, 2} = 't'; titles{6, 3} = 'a_w, a_w*';
titles{7, 1} = 'd_a(t), d_a*(t)'; titles{7, 2} = 't'; titles{7, 3} = 'd_a, d_a*';
titles{8, 1} = '(H-H*)(t), SKO(t)'; titles{8, 2} = 't'; titles{8, 3} = 'H-H*, SKO';
titles{9, 1} = '(V-V*)(t), SKO(t)'; titles{9, 2} = 't'; titles{9, 3} = 'V-V*, SKO';
titles{10, 1} = '(a-a*)(t), SKO(t)'; titles{10, 2} = 't'; titles{10, 3} = 'a-a*, SKO';
titles{11, 1} = '(w_z-w_z*)(t), SKO(t)'; titles{11, 2} = 't'; titles{11, 3} = 'w_z-w_z*, SKO';
titles{12, 1} = '(v-v*)(t), SKO(t)'; titles{12, 2} = 't'; titles{12, 3} = 'v-v*, SKO';
titles{13, 1} = '(a_w-a_w*)(t), SKO(t)'; titles{13, 2} = 't'; titles{13, 3} = 'a_w-a_w*, SKO';
titles{14, 1} = '(d_a-d_a*)(t), SKO(t)'; titles{14, 2} = 't'; titles{14, 3} = 'd_a-d_a*, SKO';
for i=1:14
figure(i);
if i < 8;
plot(t,d(i,:),'-.b', t, f(i,:), '--r'), grid on;
else
plot(t,d(i-7,:)-f(i-7,:),'-.b',t, sqrt(q(i-7,:)),'r'), grid on;
end;
title( titles{i,1} );
xlabel( titles{i,2} );
ylabel( titles{i,3} );
end;

7
Результаты моделирования:

Зависимость H от времени.

8
Зависимость V от времени.

Зависимость α от времени.

9
Зависимость ω z от времени.

Зависимость ϑ от времени.

Зависимость α ω от времени.
10
Зависимость Δ α от времени.

11
Среднеквадратическое отклонение H от времени.

Среднеквадратического отклонения V от времени.

Cреднеквадратическое отклонение α от времени.

12
Среднеквадратическое отклонение ω z от времени.

13
Среднеквадратическое отклонениеϑ от времени.

Среднеквадратическое отклонениеα ω от времени.

14
 Среднеквадратическое отклонение Δ α от времени.

Вывод
Фильтр Калмана является оптимальным по критерию минимизации
ковариационной матрицы ошибок оценивания. Использование данного фильтра
эффективно при использовании низкоточного датчика измерений. В случае применения
высокоточного датчика работа фильтра будет оказывать незначительное влияние на
конечный результат.

15