л,в, с,н,1), R,a

Решете уровнения Риккати Р=АР+РА- PHRhP + CQC, Ш=0

К(т)=

Вычисление матрицы К = РНВ"

Решение уравнения

Жа i iBB

.гЯкЙ! A-BD-KH

Случайные воздействия и шумы измерений можно сформировать с помощью датчика случайных чисел. В этом случае случайный сигнал описывается корреляционной функцией вида

аМ1-х/Д0 при х!/д<1,

О при т/Д>1,

где от - среднеквадратическое отклонение случайной величины; Д -шаг обращения к датчику случайных чи« сел. Применив преобразование Фурье к выражению для корреляционной функции, получим спектральную плотность случайного сигнала

5{(о) = од11н1ММ.

((оД/2)2

При достаточно малых Д спектральная плотность примет вид S(co) = = а2Д/.

Таким образом, приближенно можно считать, что случайный сигнал, формируемый датчиком случайных чисел при малых Д, является белым шумом.

Пример. Рассмотрим построение фильтра Калмана - Бьюси иа примере системы управления продольным движением самолета. Пусть имеем

хт = [а:, Хг Хь х = \Ъ О «г Щ, и=Ъь;

Траектории двитения x(t), z(t)

Конец

Рис. 8.2

-0,019 О

D=:[-3 -1,5 -1 -0,006].

В качестве возмущающего воздействия w(/) задай случайный вертикальный ветер, состоящий из суммы двух компонентов: постоянного ветра со скоростью 15 м/с и наложенного иа него случайного сигнала в виде белого шума. Среднеквадратическое отклонение случайного сигнала ст=2 м/с. Погрешности измерений также заданы в виде белых шумов с нормально распределенными мгновенными значениями. Средиеквадратические отклонения погрешности измерений OQ=a=o- =0,009 рад, он = 1 м. Для решения этой задачи составлена программа FILK.

с FIUC - ПРСГРМ«М РЕКНИЯ /РМЮМЯ ОЦЕНКИ ВЕКТОРА СОСТОЯНИЯ С

EXTERNAL PRA,OUT

DIMENSION А<4,4) ,В<4,1) ,С<4,1) ,D<1,4),Y<8) ,DY(8) ,PI(5), «PR<5»,4) ,TIHE<5») ,AB<8,8) ,BD(4,4) ,АВП<Н<4,4) ,CW(4,1), »ABD(4,4),H(1,4),AY(8,1),AUX(8,8)

CO»«N /P/AB,C,K

CO»«N A>/PR,L,Hl,TIIt

REAL V,«,K<4,1),KH<4,4),KV<4,1)

L=».

READ 1,<<A<I,J),J=1,4),I=1,4)

READ 1»,(<B<I,J),>=1,1),I=1,4)

READ 1»,(<C(I,J),>=1,1),I=1,4)

READ 1,((D<I,J),J=1,4),I=1,1) 1» FORMAT (E12.4) 1 FORMAT (4E12.4) NI>=8

9 FOraiAT <1E12.4)

READ 3,(РКП, 1=1,5)

3 FORMAT <5E12.4)

READ 4,(DY(I),I=1,8),(Y<I),I=1,8)

4 FORMAT (4E12.4)

READ 9,<(K(I,J),J=1,1),I=1,4) READ 1,((H(I,J),J=1,4),I=1,1) PfrlMT 1М»

FORMAT (1Нв,5Х,9»*>ТРИЦА A/)

PRINT 1,<<A(I,J),J=1,4),I=1,4)

PRINT IMl 1Ш FORMAT (lH»,5X,9»MTPVtUA B/) " PRINT 1»,(<B(I,JJ,J=1,1),I=1,4)

PRINT 1»2 ie»2 FORMAT (1Нв,5Х,9»МТРИЦА С/)

PRINT 1»,<(C<I,J),J=1,1),I=1,4)

PRINT 1»3 ie«3 FORMAT (lH»,SX,9HHATPVtUA D/)

PRINT 1,(<D<I,J),J=1,4),I=1,1)

PRINT ie»4 1004 FORMAT (1Н»,5Х,9»МТРИЦА К/)

PRINT 9,((K(I,J),J=1,1),I=1,4)

PRINT ie*5 ie*5 FORMAT (1Нв,5Х,9НМАТРИЦА И/)

PRINT 1,<<H(I,J),J=1,4),I=1,1) С «ОРМИРОВАНИЕ НАТРИЦЫ АВ

CALL GWRIl(B,Ii,BD,4,l,4)

CALL GMFiy:i(K,H,KH,4,l,4)

CALL GMSUB(A,BD,ABII,4,4)

CALL GMSUB(ABtl,KH,ABn<H,4,4)

IO 12 1=1,4

IO 12 J=l,4

AB(I,J)=A(I,J)

AB(I,J+4)=-BD(I,J)

AB<I+4,J)=KH(I,J)

AB( 1+4, J+4)=AB0KH( I, J) 12 CONTINUE

CALL RKE(».,4e.,».8,».»l,».mi,l».,Y,NIi,IER,FRA,0UT>

CALL PL0T84<FR,TIME,5e,4)

STOP

с EUHHCJEHHE ITPAEUX ЧЙСТЕЙ )TA»eH№S С

SUBROUTINE PRA (T,Y,DY)

DIMENSION A<4,4),B(4,l),C(4,l),ri(l,4),Y(8),riy(8),PI(5), »PR<»,4),TIME<»),AB(8,8),BD(4,4),ABDKH(4,4),CW(4,1), »AY(8,1),ABD(4,4),H(1,4),AUX(8,8)

DATA IY/»/

COMMON /P/AB,C,K

REM. V,W,K(4,1),KH(4,4),KV<4,1) l*=15+2»AN0RM(IY>

CALL S«PY(C,W,CW,4,1,») y=ANOR«(IY)»».»?

CALL S«PY(K,V,KV,4,1,»)

CALL GM№Ii(AB,Y,AY,8,8,l)

IO 1 1=1,4

DY(I)=AY(I,1)+CW(I,1) DY(I+4)=AY(I+4,1)+KV(I,1) 1 CONTINUE RETURN END

С П0ДПРС1П=-М*1А аводА

SUIKOUTItt OUT(T,Y,riY,lH,NIi,PI)

IiIMENSION YC8) ,riY(8) ,FI (5) ,PRC5«,4) ,AUX(8,8> ,Т1Ме(50) COMMON /0/PR,L,Hl,TIME IF(T-Hl) 6,5,5 5 L=L+1

IF(L-51) 2»,21,21 21 F-I(5)=3.» GOTO e,

2» PRINT 15,T,(Y(I>,I=1,8) 15 FORMAT (1H,9E12.4)

TIME(L)=T

Hl=Hl+*.2

DO 7 1=1,4

I2=I»2 7 F1«(L,r)=Y(I2) • 6 RETURN

С ДАТЧИК ССЧАЙШХ ЧИСЕЛ С

FUNCTION URMOldY)

ЮЛТА М/1»24/,АН/1»24./,«2/2»48/,Н4/4в96/,Ма/В192/

IY=IY»13+217

IF(IY.GT.M8) IY=IY-M8

IF(IY.GT.Ii4) IY=IY-li4

IF(IY.GT.M2) IY=IY-H2

IF(IY.GT.M) IY=IY-M

URANri=fLDAT<IY)/AM

RETURN

FUNCTION MWRMdY) A=«.

DO 5 1=1,12 5 A=URAND(IY)-A ANORM=A RETURN END