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

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

УЧРЕЖДЕНИЕ ВЫСШЕГО ОБРАЗОВАНИЯ
« МОСКОВСКИЙ АВИАЦИОННЫЙ ИНСТИТУТ
(НАЦИОНАЛЬНЫЙ ИССЛЕДОВАТЕЛЬСКИЙ УНИВЕРСИТЕТ)» (МАИ)
Кафедра № 301
«Системы автоматического и интеллектуального управления»

Отчёт по лабораторной работе №2

Студенты: Вишневский Д.Н.

Молчанов Н.М.

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

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

Уравнения движения
g
Δ β̇= Ź β ( Δβ+ Δ β ω ) +sin α 0∗Δ ω x +cos α 0∗Δ ω y + ∗cos ϑ0 Δγ+ Ź δ ∗Δ δ H ,
H

{
V0
Δ ω̇ x = Ḿ x ( Δβ + Δ β ω ) + Ḿ x Δ ω x + Ḿ x Δ ω y + Ḿ δx Δδ э + Ḿ δx Δ δ H ,
β ω
x ωy э H

Δ ω̇ y = Ḿ βy ( Δβ + Δ β ω )+ Ḿ ωy Δ ω x + Ḿ ωy Δω y + Ḿ δy Δδ э + Ḿ δy Δ δ H ,
x y э H

Δ γ̇= Δ ω x −tgϑ 0∗Δω y ,

1
Δ ψ̇ p= ∗Δ ω y ,
cos ϑ0
−1 180 180
Δ β̇ ω= ∗Δ β w + ∗Δ β + η ,
T πT V 0 πT V 0 β
Δ̇ β =0.

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

 Z – горизонтальная координата, отсчитываемая перпендикулярно от линии
заданного пути;
 η β – белый шум;
 Ź β – производная приведённой к произведению массы БПЛА на скорость
установившегося полёта аэродинамической подъёмной силы крыла по β;
 Ź δ – производная приведённой к произведению массы БПЛА
H

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

подъёмной силы по δ H ;
β β
 Ḿ x , Ḿ y – производные приведённых к соответствующему моменту инерции
моментов γ и ψ p по β ;
ω ω
 Ḿ x , Ḿ y – приведённые к соответствующему моменту инерции демпфирующие
y x

моменты γ и ψ p.
δ δ δ δ
 Ḿ x , Ḿ y , Ḿ x , Ḿ y – производные приведённых к соответствующему моменту
э э H H

инерции моментов γ и ψ pпо δ э и δ H ;

 Δ β w , Δ β – приращение угла и систематической составляющей скорости бокового
ветра.

Управление по элеронам и рулю направления

U э =2 ( ϑ−ϑ зад ) +0 . 5 ω x
{ U н=0.1ϑ+ 0.6 ω y

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

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 ,
2
где G - матрица эффективности шумов, P- матрица ковариаций, F - динамическая матрица
объекта, B – матрица эффективности управления, R - матрица шумов в канале измерений,
H - Матрица измерений, ^x k - оценка.

G
v

x’
z
u x
B 1/S H

p
K

p
x^∙
x^
B 1/S H

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

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

100 м/с M δxH

-0,2
β
M x
-1,2 M δyэ
-0.1
β
M y
-2 M δyH
-3
ωx
M y
0 T -0,2 c.
M ωy
y
-0,8 Δt 0,01с.
M ωy
-0.7 ϑ0 2°
x
ωx
-0,6 θ 20°
M x
δэ
α0 2°
M x
-4
3
Начальная матрица ковариаций (дисперсии ошибок оценивания) P:

35 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;
0000005

Матрица измерений 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,01 0 0
0 0 0 0,0008 0
0 0 0 0 0,01

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

-2 0,035 0,999 0,098 0 -2 0

-1,2 -0,60 0 0 0 -1,20 0
-2 0 -0,800 0 0 -2 0
0 1 -0,035 0 0 0 0
0 0 1,0006 0 0 0 0
0 0 0 0 0 -5 2,865
0 0 0 0 0 0 0

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

0 -0,002
-0,040 -0,002
-0,001 -0,030
0 0
0 0
0 0
0 0

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

0
0
0
0
0
0,0286
4
0

Расчёты в MATLAB
% ----------------------------------------------------------------------
% Компьютерные технологии управления в технических системах
% Лабораторная работа # 2
% Построить оптимальный фильтр Калмана для бокового движения самолета
clc, clear;
% Начальные условия
% ----------------------------------------------------------------------
nn = 10; % время полета, сек.
d_t = 0.01; % сек.
t = 0 : d_t : nn;
n = length(t);
Vo = 100; % начальная скорость, м/с
T = 0.2; % постоянная времени формирующего фильтра, сек.
teta = pi / 180 * 2;
alpha = pi / 180 * 2;
gamma_z = 20;
k_omega_x = 0.5;
k_omega_y = 0.6;
k_gamma = 2;
k_beta = 0.1;
% производная приведённой к произведению массы БПЛА на скорость
% установившегося поле?та аэродинамическои? подъе?мнои? силы крыла по ?
Z_beta = -2;
% производная приведённой к произведению массы БПЛА
% на скорость установившегося поле?та аэродинамическои?
% подъе?мнои? силы по ?_H
Z_delta_h = -0.2;
% производные приведённых к соответствующему моменту
% инерции моментов ? и ?_p по ?
M_beta_x = -1.2;
M_beta_y = -2;
% приведённые к соответствующему моменту инерции
% демпфирующие моменты ? и ?_p
M_omega_x = -0.6;
M_omega_y = -0.8;
% производные приведённых к соответствующему моменту
% инерции моментов ? и ?_pпо ?_э и ?_H
M_delta_e_x = -4;
M_delta_e_y = -0.1;
M_delta_h_x = -0.2;
M_delta_h_y = -3;
% Динамическая матрица объекта
A = [Z_beta sin(alpha) cos(alpha) (9.8*cos(teta))/Vo 0 Z_beta 0;
M_beta_x M_omega_x 0 0 0 M_beta_x 0;
M_beta_y 0 M_omega_y 0 0 M_beta_y 0;
0 1 -tan(teta) 0 0 0 0;
0 0 1/cos(teta) 0 0 0 0;
0 0 0 0 0 -1/T 180 / (pi * T * Vo);
0 0 0 0 0 0 0];
% Матрица эффективности управления
B = [
0 Z_delta_h;
M_delta_e_x M_delta_h_x;
M_delta_e_y M_delta_h_y;
0 0;
0 0;
0 0;
0 0];
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]';
% Матрица ковариаций
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;

5
 0 0 0 0 0 25 0;
0 0 0 0 0 0 5];
% Матрица шумов в канале измерений
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 0]';
% Проверка на наблюдаемость по Калману
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);
% ----------------------------------------------------------------------
% Построение
% ----------------------------------------------------------------------
x_o=x;
for i =1:n
% Управление
u = [
k_gamma * (x(4) - gamma_z) + k_omega_x * x(2);
k_beta * x(1) + k_omega_y * x(3);
];
% Вычисление вектора состояния на следующем шаге
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} = 'beta(t), beta*(t)'; titles{1, 2} = 't'; titles{1, 3} = 'beta, beta*';
titles{2, 1} = 'w_x(t), w_x*(t)'; titles{2, 2} = 't'; titles{2, 3} = 'w_x, w_x*';
titles{3, 1} = 'w_y(t), w_y*(t)'; titles{3, 2} = 't'; titles{3, 3} = 'w_y, w_y*';
titles{4, 1} = 'gamma(t), gamma*(t)'; titles{4, 2} = 't'; titles{4, 3} = 'gamma, gamma*';
titles{5, 1} = 'psi(t), psi*(t)'; titles{5, 2} = 't'; titles{5, 3} = 'psi, psi*';
titles{6, 1} = 'beta_w(t), beta_w*(t)'; titles{6, 2} = 't'; titles{6, 3} = 'beta_w, beta_w*';
titles{7, 1} = 'd_b(t), d_b*(t)'; titles{7, 2} = 't'; titles{7, 3} = 'd_b, d_b*';
titles{8, 1} = '(beta-beta*)(t), SKO(t)'; titles{8, 2} = 't'; titles{8, 3} = 'beta-beta*, SKO';
titles{9, 1} = '(w_x-w_x*)(t), SKO(t)'; titles{9, 2} = 't'; titles{9, 3} = 'w_x-w_x*, SKO';
titles{10, 1} = '(w_y-w_y*)(t), SKO(t)'; titles{10, 2} = 't'; titles{10, 3} = 'w_y-w_y*, SKO';
titles{11, 1} = '(gamma-gamma*)(t), SKO(t)'; titles{11, 2} = 't'; titles{11, 3} = 'gamma-gamma*, SKO';
titles{12, 1} = '(psi-psi*)(t), SKO(t)'; titles{12, 2} = 't'; titles{12, 3} = 'psi-psi*, SKO';
titles{13, 1} = '(beta_w-beta_w*)(t), SKO(t)'; titles{13, 2} = 't'; titles{13, 3} = 'beta_w-beta_w*, SKO';
titles{14, 1} = '(d_b-d_b*)(t), SKO(t)'; titles{14, 2} = 't'; titles{14, 3} = 'd_b-d_b*, 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} );

6
end;

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

На рисунках 2-8 изображены зависимости различных величин от времени. На них обозначены графики
изменения вектора состояния (обозначены пунктиром с точкой) и оценки (обозначены пунктиром).

Рис. 2. Зависимость β от времени. Рис. 3. Зависимость ω x от времени.

Рис. 4. Зависимость ω y от времени. Рис. 5. Зависимость γ от времени.

Рис. 6. Зависимость ψ p от времени. Рис. 7. Зависимость β ω от времени.

8
 Рис. 8. Зависимость Δ β от времени.

На рисунках 9-15 изображены графики среднеквадратических отклонений. Гладкие прямые линии

представляют собой графики среднеквадратических отклонений в зависимости от времени. Пунктирные
линии – графики зависимостей разниц между векторами состояний и оценками от времени.

Рис. 9. График среднеквадратического отклонения β. Рис. 10. График среднеквадратического отклонения ω x .

Рис. 11. График среднеквадратического отклонения ω y . Рис. 12. График среднеквадратического отклоненияγ .

9
 Рис. 13. График среднеквадратического отклоненияψ p. Рис. 14. График среднеквадратического отклонения β ω..

Рис. 15. График среднеквадратического отклонения Δ β ..

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

10