Вычислительные методы алгебры и оценивания. И.В. Семушкин (2011) (1185350), страница 46

Просмтор этого файла доступен только зарегистрированным пользователям. Но у нас супер быстрая регистрация: достаточно только электронной почты!

Текст из файла (страница 46)

Аддитивно действующий белый шум с единичной ковариационной матрицей Q(ti) = I обозначенчерез wk−1, а ∆t — период дискретизации времени. Ковариация шума Γwравна [114]cov(Γw) , E ΓwwT ΓT = ΓΓT = diag"0 00 0, ∆t2σv̇2 ,"∆t33∆t22∆t222∆t##σω̇2 ,где σv̇2 и σω̇2 — дисперсии случайных гауссовских процессов, аддитивно воздействующих на скорость корабля и его угловую скорость, соответственно.Модель движения (13.18) включает угловую скорость и потому пригоднадля анализа траектории как маневрирующего судна, так и судна, движущегося равномерно и прямолинейно (в последнем случае угловая скоростьсудна-цели равна нулю).Модель измеренийБудем считать, что наблюдения за судном ведутся при помощи радиолокационной станции (РЛС), возвращающей измерения сферических (на плоскости — полярных) координат цели: азимут zθ и дальность до цели zρ .Построим модель измерения в декартовых координатах.

Будем называтьтакие преобразованные измерения псевдоизмерениями. Первичные измереzρния, приходящие от РЛС, обозначим zsph =. Тогда zρ = ρ + δρ ,zθzθ = θ + δθ , где ρ и θ — истинные значения дальности и пеленга, а δρ иδθ — соответствующие погрешности измерения. Обозначим шум измеренийδρполярных координат vsph =. Его ковариационная матрица имеет видδθ 2σ0ρT=Rsph , E vsphvsph, т. е.

матрица диагональна, а шумы по даль0 σθ2ности и азимуту взаимно не коррелированы (ошибки измерения дальностине связаны с ошибками измерения направления на цель).29413.12 Задача сопровождения судна на траекторииПерейдем от измерений в сферической (точнее, полярной) системе координат к псевдоизмерениям в декартовых координатах: zxzρ cos(zθ )zdec ==.zyzρ sin(zθ )Для удобства используем здесь и далее индекс «dec» в случаях работы сдекартовыми координатами. Малые приращения координат в сферическойи декартовой системе координат связаны соотношением: dzxcos(zθ ) −zρ sin(zθ )dzρdzdec ==.dzydzθsin(zθ ) zρ cos(zθ )cos(zθ ) −zρ sin(zθ )Обозначим T =. Перейдем от дифференциалов кsin(zθ ) zρ cos(zθ )конечным разностям и получим закон преобразования случайных погрешностей измерений из полярных координат в декартовы: ∆zxcos(zθ ) −zρ sin(zθ )∆zρ∆zdec === T ∆zsph .∆zy∆zθsin(zθ ) zρ cos(zθ )Ковариационная матрица погрешностей измерений в декартовых координатах равнаTT= E T ∆zsph ∆zsphT T = T Rsph T T .Rdec , E ∆zdec ∆zdecМатрица T здесь зависит от времени, однако для простоты изложения индекс времени опускаем.Выразим псевдоизмерения через истинные декартовы координаты цели ипогрешности псевдоизмерений: zρ cos(zθ )(ρ + ∆ρ) cos(θ + ∆θ)zdec ==≈zρ sin(zθ )(ρ + ∆ρ) sin(θ + ∆θ) ρ cos(θ) + ∆zxx∆zx≈=+.ρ sin(θ) + ∆zy∆zyy xЗдесь обозначим: s =— точные значения декартовых координат цели,y∆zxv=– погрешности псевдоизмерений декартовых координат.

Тогда∆zyzdec = s+v . Вследствие нелинейности преобразования координат ковариационная матрица погрешностей псевдоизмерений Rdec не является диагональной. Приведем ковариационную матрицу к единичному виду, т. е. выполнимдекорреляцию погрешностей псевдоизмерений.29513 Устойчивые алгоритмы фильтрацииT, где S =Представим матрицу Rdec как произведение Rdec = Sdec Sdec1/2−1−1−1−1−1= T Rsph . Обозначим z̃dec = Sdec zdec = Sdec (s + v) = Sdec s + Sdec v = Sdecs + ṽ. −1 T −1 T T== E Sdec v v (Sdec )Найдем ковариацию ṽ: cov(ṽ) = R̃dec , E ṽ ṽ−1−T= Sdec Rdec Sdec = I.

Будем называть z̃dec нормализованными псевдоизмерениями, а ṽ — вектором погрешностей нормализованных псевдоизмерений(его компоненты взаимно не коррелированы, так как R̃dec = I ). Отсутствиекорреляции компонентов вектора погрешностей позволяет применять скаляризованную обработку измерений.Поскольку уравнения в системе (13.18) обладают значительной нелинейностью, непосредственное применение линейного фильтра Калмана, CKF,не представляется возможным. Одним из способов обойти это ограничениеявляется использование расширенного фильтра Калмана (Extended KalmanFilter, EKF).Стандартный алгоритм расширенного фильтра КалманаРасширенный фильтр Калмана является субоптимальным алгоритмом[90].

В области фильтрации и идентификации систем этот термин не обладает уникальностью смысла. В терминологии данного подраздела «расширенность» в сравнении с линейным фильтром состоит в возможности принятия нелинейной модели движения цели и/или модели измерений. В данном контексте это — общепринятая терминология. Формулы расширеннойфильтрации получены в работе [128] методом инвариантного погружения.Матрицы Φ и H, задающие линейные преобразования, заменены функциями, которые в общем случае могут быть нелинейными. Это означает, чтофункционирование системы описывается следующими уравнениями:x(ti+1) = f [x(ti)] + w(ti) ,z(ti ) = h[x(ti)] + v(ti) .Функция f [·] вычисляет состояние системы в момент времени ti+1 посостоянию в момент времени ti , функция h[·] преобразует вектор состоянияк виду, в котором измерения поступают на вход фильтра, w(ti) и v(ti) —шум процесса и шум измерения в момент ti , соответственно.

Когда обефункции f [·] и h[·] — линейные, расширенный фильтр превращается встандартный фильтр Калмана.29613.12 Задача сопровождения судна на траекторииРасширенный фильтр Калмана, как и стандартный, содержит два этапа:этап 1 — экстраполяция и этап 2 — обработка измерения.Экстраполяция (здесь учтено, что Q(ti) = I):x̂(ti) = f [x̂(t+i−1)] ,∂f F (ti−1) =,∂x x=x̂(t+i−1 )+TTP (t−i ) = F (ti−1 )P (ti−1 )F (ti−1 ) + Γ(ti−1)Γ (ti−1 ) .Обработка измерения (здесь учтено, что R(ti ) = R̃dec = Im):ν(ti) = z(ti ) − h[x̂k (t−i )] ,∂h ,H(ti) =∂x x=x̂(t+i−1 )−TT−1K(ti) = P (t−,i )H (ti )[H(ti )P (ti )H (ti ) + Im ]−−P (t+i ) = P (ti ) − K(ti )H(ti )P (ti ) ,−x̂(t+i ) = x̂(ti ) + K(ti )ν(ti) .Приведенные уравнения расширенной фильтрации по структуре совпадают с уравнениями обычного, линейного фильтра.

Это — следствие того,что в них сохранены только первые (линейные) члены разложения функций f [·] и h[·] в ряды Тейлора. В силу этого, рассматриваемый расширенный фильтр — линейный (так называемый расширенный фильтр первогопорядка).Линеаризация моделей процесса и измерений производится относительнопоследней (текущей) оценки вектора состояния. Для этого вычисляют двематрицы Якоби: F (ti−1) и H(ti), которые затем подставляют в уравнениястандартной калмановской фильтрации.Эффективная реализация расширенного фильтраПокажем, как применять рассмотренные выше квадратно–корневые реализации фильтра Калмана к задаче фильтрации траектории судна–цели врасширенном фильтре. Проделаем это на примере одной из реализаций —на примере фильтра Поттера, хотя могут быть использованы другие.29713 Устойчивые алгоритмы фильтрацииПусть на вход фильтра поступают нормализованные псевдоизмерения(обладающие, как показано выше, единичной ковариационной матрицейошибок).

Для модели процесса, описываемой уравнениями (13.18), функцииf [·] и h[·] имеют следующий вид:f [s(ti)] = x + (2/ω) v sin(ω∆t/2) cos(φ + ω∆t/2)y + (2/ω) v sin(ω∆t/2) sin(φ + ω∆t/2)vφ + ω∆tωh[s(ti )] =xy ,ti.tiОбозначим: ϕ = φ + ω∆t . Вычислим матрицы Якоби:∂f ,F =F (ti−1) ,∂x x=x̂(t+i−1 )1 0 sin(ϕ)−sin(φ)ωcos(φ)−cos(ϕ)0 1ω=0 010 000 00v(cos(ϕ)−cos(φ))ωv(sin(ϕ)−sin(φ))ω∂h ≈H(ti ) ,∂x x=x̂(t+i−1 )010v(∆t ω cos(ϕ)+sin(φ)−sin(ϕ))ω2v(∆t ω sin(ϕ)+cos(ϕ)−cos(φ))ω21 0 0 0 00 1 0 0 00∆t1,t+i−1=H.Поскольку нормализованные псевдоизмерения на входе фильтра «производятся» (т.

е. формально представлены) в той же системе координат, вкоторой записаны координаты объекта в векторе состояния, функция h[·]выполняет линейное преобразование, и матрица H(ti ) = H не зависит отвремени. Приближенность записи H(ti ) = H здесь состоит в том, что в условиях малости величин ∆zsph косинус угловой погрешности заменен на 1.При прямолинейном равномерном движении (ω = 0) функции f [·] и Fпринимают следующую форму:29813.12 Задача сопровождения судна на траекторииx + (2/ω) v sin(ω∆t/2) cos(φ + ω∆t/2) y + (2/ω) v sin(ω∆t/2) sin(φ + ω∆t/2)  =fω=0[s(ti)] =lim vω→0 φ + ω∆tωtix + v∆t cos(φ) y + v∆t sin(φ)  ,=vφ0tiv ∆t2 sin(φ)1 0 ∆t cos(φ) −v ∆t sin(φ) −2v ∆t2 cos(φ)  0 1 ∆t sin(φ) v ∆t cos(φ) −2Fω=0(ti−1) =  0 0 .1000 001∆t0 0001t+i−1Конкретные значения элементов F во время фильтрации получаютсяподстановкой сюда элементов соответствующего вектора оценки состояния,однако для упрощения записей в последующих формулах символы экстраполяционной оценки (тильда ˜) и отфильтрованной оценки («крышка» ˆ)опущены над всеми входящими величинами.Учывая изложенное, скаляризованный алгоритм Поттера для нелинейнойфильтрации данных о траектории судна–цели получим в следующем виде:I.

Экстраполяция:x + (2/ω) v sin(ω∆t/2) cos(φ + ω∆t/2) y + (2/ω) v sin(ω∆t/2) sin(φ + ω∆t/2) v , ŝ(t+ ) = s0 ,ŝ(t−)=0i φ + ω∆tωt+isin(ϕ)−sin(φ) v(cos(ϕ)−cos(φ)) v(∆t ω cos(ϕ)+sin(φ)−sin(ϕ))1 0ωωω2cos(φ)−cos(ϕ) v(sin(ϕ)−sin(φ)) v(∆t ω sin(ϕ)+cos(ϕ)−cos(φ)) 0 1ωωω2F (ti , ti−1) =  0 0 ,1000 001∆t0 0001t+i−129913 Устойчивые алгоритмы фильтрацииS T (t−i )0=TTS T (t+i−1 )F (ti , ti−1 )ΓT (ti ).II. Обработка измерения:( −А.

Характеристики

Список файлов книги