В.В. Александров, С.С. Лемак, Н.А. Парусников - Лекции по механике управляемых систем, страница 14

. . , zj−1 (априорная оценка x̃−j = M [xj |z0 , z1 , ...zj−1 ]);+x̃j — оценка вектора x в момент времени j, использующая измерения z0 , . . . , zj−1 , zj (апостериорная оценка x̃+j = M [xj |z0 , z1 , ...zj ]);91Pj− — ковариация ошибки априорной оценки вектора x в моментj;Pj+ — ковариация ошибки апостериорной оценки вектора x в момент j.Для каждого момента времени j, j = 0, 1, . . . задача решена налекции 12, откуда следует, что процедуру оценивания можно представить в виде повторяющихся от момента к моменту циклов, каждый изкоторых состоит из двух этапов — этапа коррекции и этапа прогноза.Этап коррекции — переход от априорных оценок к апостериорным в момент j. В соответствии с формулами (12.9)−−x̃+j = x̃j + Kj (zj − Hj x̃j ),Kj = Pj− Hj (Hj Pj− Hj + Rj )−1 ,Pj+= (E −(13.1)Kj Hj )Pj− .Этап прогноза — переход от апостериорных оценок x̃+ в моментj к априорным оценкам x̃− в момент j + 1.

Имеем+x̃−j+1 = Φj x̃j ,−= Φj Pj+ ΦPj+1j + Qj .(13.2)Кроме того, для включения начального цикла надо положитьx̃−0 = x̃0 ,P0− = P0 .(13.3)Алгоритм оценивания, описываемый соотношениями (13.1)–(13.3),носит название дискретного фильтра Калмана.Пояснение.

Из самой процедуры получения оценок x̃+j следует,что ошибка оценки Δxj = xj − x̃+ортогональнатекущемуизмерениюjzj :◦◦M [Δxj z j ] = 0, z j = zj − Hj x̃−j .Но она также ортогональна всем предшествующим измерениям. Этот факт проиллюстрируем на примере первых двух измерений.Оценка x̃+0 в начальный момент строится из условия◦◦M [Δx0 z 0 ] = 0, Δx0 = x0 − x̃+0 , z 0 = z0 − H0 x̃0 ,где x̃0 — априорное математическое ожидание величины x0 .Имеемx1 = Φ0 x0 + q0 , z1 = H1 x1 + r1 .Оценка x̃+1 в момент времени t1 строится из условия◦++−−M [Δx1 z 1 ] = 0, x̃−1 = Φ0 x̃0 , x̃1 = x̃1 + K1 (z1 − H1 x̃1 ).92Покажем, что величина Δx1 ортогональна измерению z0 .

Для этого ошибка Δx1 должна иметь следующую структуру:Δx1 = A1 Δx0 + A2 q0 + A3 r1 .Из выше написанного следует−Δx1 = x1 − x̃−1 − K1 z1 + K1 H1 x̃1 ;+x1 − x̃−1 = Φ0 x0 + q0 − Φ0 x̃0 = Φ0 Δx0 + q0 ;−z1 − H1 x̃−1 = H1 x1 + r1 − H1 x̃1 == H1 (x1 − x̃−1 ) + r1 = H1 (Φ1 Δx0 + q0 ) + r1 .Таким образом, мы показали, что величина Δx1 имеет требуемуюструктуру. Отсюда по индукции следуетΔxj ⊥ zj−1 ⊥ zj−2 ⊥ . . . ⊥ z0 .Возможно обобщение алгоритма дискретного фильтра Калмана наслучай коррелированности шумов системы и шумов измерений.

Пустьдополнительно M [qj rjT ] = Lj , тогда соотношения, описывающие этапкоррекции, остаются прежними, а ковариационное соотношение этапапрогноза будет таким− Pj+1= Φj Pj+ Φj + Q j − Φj K j L j − L j K j Φj .(13.4)В частном случае, нередко встречающемся на практике, приqj = qj + Ψj rj ,M [qj rj ] = 0,получим− = Φj Pj+ ΦPj+1j +Qj +Ψj Rj Ψj −Φj Kj Rj Ψj −Ψj Rj Kj Φj , (13.5)гдеQj = M [qj qj ].2. Некоторые свойства дискретного фильтра КалманаРассмотрим стационарный случай: Φj = Φ = const , Qj = Q =const , Rj = R = const . Поскольку Q ≥ 0, то существует представление (неоднозначное) Q = BB , где матрица B называется квадратным корнем из матрицы Q.Теорема 16.

Пусть пара (Φ, H) наблюдаема, пара (Φ, B) управляема. Тогда при бесконечном времени наблюдения:−+, P∞, K∞ , определяемые соотношениями1) существуют P∞−+ P∞= ΦP∞Φ + Q,93+−−P∞= P∞− K∞ HP∞,− − K∞ = P∞H (HP∞H + R)−1 ;−2) уравнения ошибок относительно величин Δx−j = xj − xj ,+Δx+j = xj − xj+Δx−j+1 = ΦΔxj + qj ,−Δx+j = (E − K∞ H)Δxj − K∞ rjтаковы, что при qj = 0, rj = 0 выполнено+Δx−j , Δxj → 0,(j → ∞).Более подробно случай стационарности будет рассмотрен в разделе, посвященном непрерывному фильтру Калмана.Пример 13.1. Пустьxj+1 = xj + qj ,zj = xj + rj ,Здесь Φ = 1, H = 1, Kj =Q = 1,Pj−(Pj− +3/4)значение K и вид фильтра:31−+P∞= ,= ,P∞22+x̃−j+1 = x̃j ,R=3.4.

Найдем установившеесяK∞ =2,32−−x̃+j = x̃j + (zj − x̃j ).3Уравнение ошибок таково:112Δx+j + qj − rj ,333при этом однородное уравнение1+Δx+j+1 = Δxj3асимптотически устойчиво.Полученные результаты, связанные с дискретным фильтром Калмана, озволяют придать иную форму рассмотренному ранее (см. лекцию 11) методу наименьших квадратов в задаче решения переопределенной системы алгебраических уравнений z = Hx.Рассматривая компоненты zj как следующие одно за другим измерения, методу наименьших квадратов можно придать рекуррентнуюΔx+j+1 =94форму, интерпретируя индекс j как дискретное время.

Будем считать,что в начальный момент j = 0. В качестве начальной ковариации вектора x примем величину P0 = κE, где κ — достаточно большое число(κ 1). Далее используем процедуру фильтра Калмана, исключив изнеё этап прогноза. Будем считать, что все измерения некоррелированы между собой и стохастически равноточны. Положим для определенности, что M [rj2 ] = 1.Первый шаг:0x̃1 = x̃0 + K1 (z1 − h1 x̃ ),−1 P1 = P0 − P0 h1 (hh1 P0 ,1 P0 h1 + 1)−1K1 = P0 h1 (h.1 P0 h1 + 1)Второй шаг:(1)),x̃2 = x̃1 + K2 (z2 − h2 x̃−1 P2 = P1 − P1 h2 (hh2 P1 ,2 P1 h2 + 1)−1K2 = P1 h2 (h.2 P1 h2 + 1)......j-ый шаг:j−1),x̃j = x̃j−1 + Kj (zj − hj x̃−1 hj Pj−1 ,Pj = Pj−1 − Pj−1 hj (hj Pj−1 hj + 1)−1Kj = Pj−1 hj (h.j Pj−1 hj + 1)Последний шаг при j = m.√Очевидно, что при достаточно большой величине κ, x̃(m) с высокой степенью точности совпадает с оценкой x̃ = (H H)−1 H z.3.

Реализация дискретного фильтра Калмана методом квадратного корня.На практике обычно используется численная модификация дискретного фильтра Калмана, основанная на так называемом методеквадратного корня.Метод квадратного корня состоит в том, что ковариационная матрица P представляется либо в виде разложения P = SS , где S —верхне- или нижнетреугольная матрица, либо в виде P = U DU , гдеU – верхне- или нижнетреугольная матрица с единицами на главнойдиагонали, D = diag (d1 , d2 , ...dn ), причем dj > 0. При таком представлении, каковы бы ни были невырожденные матрицы S и D, матрица P будет положительно определенной.95Метод корня в частности позволяет избежать потери положительной определенности ковариационной матрицы P на этапе коррекциипри вычитании, когда разности соизмеримы с квантом дискретизациипо уровню.

Далее рассматривается только вариант, когда P представляется в виде P = SS T .В соответствии со сказанным, всюду в соотношениях, описывающих фильтр Калмана, заменим ковариацию P на произведение SS ,где S = P 1/2 — треугольная матрица.Рассмотрим сначала случай скалярного измерения:z j = hj xj + rj ,M [rj ] = 0,M [rj ri ] = Rj δji .Этап коррекции (нижние индексы пока опускаем). ИмеемP + = P − − P − h(h P − h + R)−1 h P − ,x̃+ = x̃− + K(z − h x̃− ),K = P − h(h P − h + R)−1 .Положим P − = S − S − , P + = S + S + .

ПолучимS + S + = S − S − − S − S − h(h S − S − h + R)−1 h S − S − ,K = S − S − h(h S − S − h + R)−1 .Введем обозначения f = S − h, α = f f + R. Окончательно получим1/211,K = S − f.(13.6)S+ = S− E − f f ααЕсли z — вектор и матрица R — диагональна (составляющиевектора r некоррелируемы между собой), этап коррекции реализуется путем последовательной поскалярной обработки каждого измерения, причем выходное (апостериорное) значение S и x каждого шагаслужит входной информацией следующего шага.

Результат обработкипоследней координаты вектора z служит исходной информацией дляперехода к этапу прогноза.Замечание. Если матрица R не диагональна, то невырожденнымпреобразованием C вводится векторz ∗ = Cz = CHx + Cr = H ∗ x + r∗такой, что координаты вектора r∗ некоррелируемы.Например, матрицу C можно выбрать так, чтобы M [r∗ r∗ ] = E.Имеем в этом случае M [r∗ r∗ ] = CRC = E. Откуда C = R−1/2 —обратная матрица к матрице квадратного корня R1/2 .96Этап прогноза.+−+ x−j+1 = Φj xj , Pj+1 = Φj Pj Φj + Qj ,или−−++ Sj+1= Φj Sj+1Sj+1Φj + Q j .Sj+1+.

ТогдаОбозначим Wj = Φj Sj+1−Sj+1= [Wj Wj + Qj ]1/2 .97Лекция 14Непрерывный фильтр КалманаРассматривается непрерывная линейная динамическая система,поведение которой описывается вектором состояния(14.1)ẋ = A(t)x + q.Возмущения q(t) представляют собой нормальный случайный процесс типа белого шума:M [q(t)] = 0,M [q(t)q (s)] = Q(t)δ(t − s),Q(t) ≥ 0.Заданы также начальные условия:x̃(t0 ) = M [x(t0 )], P (t0 ) = M {x(t0 ) − x̃(t0 )} {x(t0 ) − x̃(t0 )} ≥ 0.Возмущение q(t) предполагается независимым от начального состояния системы x(t0 ):M [x(t0 )q (t)] = 0,∀t ≥ t0 .Пусть информация(14.2)z(t) = H(t)x(t) + r(t)поступает непрерывно на интервале [t0 , t].Погрешность информации r(t) — случайный нормальный процесстипа белого шума:M [r(t)r (s)] = R(t)δ(t − s),R(t) > 0.Кроме тогоM [x(t0 )r (t)] = 0,∀t ≥ t0иM [r(t)q (s)] = 0,∀ t, s.Требуется в любой момент времени t > t0 определить оценку x̃(t),удовлетворяющую условиям линейности, несмещенности и ортогональности.1.

Линейность. Оценку будем искать в видеx̃˙ = Γ1 x̃ + Γ2 z,где матрицы Γ1 и Γ2 подлежат выбору.2. Hесмещенность. Запишем уравнение ошибок оценки относительно величины Δx = x − x̃:Δẋ = Γ1 Δx + (A − Γ1 − Γ2 H)x + q − Γ2 r.98Требование M [Δx] = 0 приводит к условиюA − Γ1 − Γ2 H = 0илиΓ1 = A − KH,K = Γ2 .Таким образом, уравнение для оценки x имеет видx̃˙ = Ax̃ + K(z − H x̃).Матрица K подлежит определению.3. Ортогональность.

Условие ортогональности имеет вид◦M [Δx(t)z (s)] = 0,t0 ≤ s ≤ t,где Δx служит решением уравнения ошибокΔẋ = (A − KH)Δx + q − Krи(14.3)◦z(s) = z(s) − M [z(s)] = z(s) − H x̃ = H(s)Δx + r(s).ОтсюдаPx (t, s)H (s) + M [Δx(t)r (s)] = 0,(14.4)где Px (t, s) = M [Δx(t)Δx (s).Запишем решение уравнения (14.3):tΔx(t) = Φk (t, t0 )Δx(t0 ) + Φk (t, τ ) [q(τ ) − K(τ )r(τ )] dτ,t0где Φk (t, t0 ) — переходная матрица соответствующей линейной системы.Далее получимtM [Δx(t)r (s)] = − Φ(t, τ )K(τ )M [r(τ )r (s)]dτ =t0t=−Φ(t, τ )K(τ )R(τ )δ(τ − s)dτ = −Φ(t, s)K(s)R(s).t0Уравнение (14.4) теперь запишется так:Px (t, s)H (s) − Φ(t, s)K(s)R(s) = 0.Устремляя s → t, находим K(t):K(t) = Px (t)H (t)R−1 (t).99Ковариационная матрица Px (t) = M [Δx(t)Δx (t)] находитсякак решение дисперсионного уравнения, соответствующего уравнению (14.3):Ṗx (t) = (A(t) − K(t)H(t))Px (t)++ Px (t)(A(t) − K(t)H(t)) + Q(t) + K(t)R(t)K (t).(14.5)Подставив выражение для матричного коэффициента усиления Kв (14.5), получимṖx (t) = A(t)Px (t) + Px (t)A (t) + Q(t)−− Px (t)H (t)R−1 (t)H(t)Px (t).(14.6)Уравнение вида (14.6) в теории дифференциальных уравнений носитназвание уравнения Риккати.Алгоритм оценивания, описываемый соотношениями˙x̃(t)=A(t)x̃(t) + K(t)(z(t) − H(t)x̃(t)), x̃(t ) = x̃ ,00K(t) =Px (t)H (t)R−1 (t),Ṗx (t) =A(t)Px (t) + Px (t)A (t) + Q(t)−Px (t0 ) = P0(14.7)− Px (t)H (t)R−1 (t)H(t)Px (t),носит название непрерывного фильтра Калмана.Алгоритм, доставляющий оценку вектора состояния в соответствии с формулами (14.7), вырождается при R = 0 (т.е.