Биард Р.У., МакЛэйн Т.У. Малые БЛА - теория и практика (2015) (1245764), страница 31
Текст из файла (страница 31)
Ковариация R может быть без труда оценена по результатамкалибровки датчика, но ковариация Q в общем случае неизвестна и поэтомустановится параметром системы, который можно надстраивать для улучшениярабочих характеристик наблюдателя. Обратите внимание, что частота дискретизации не требует фиксации.Аналогично уравнениям (8.15) и (8.16) дискретно-непрерывный фильтрКалмана имеет видx&$ = A x$ + Bu,x$+ = x$ + L(y(tn) C x$).$ Ковариация ошибки измерения вОпределим ошибку оценки как ~x = x x.момент времени t дается выражениемP(t) @ E{ ~x (t) ~x (t)Т}.(8.20)Заметьте, что P(t) — симметричная и положительная полуопределеннаяматрица, поэтому ее собственные числа действительные и неотрицательные.Кроме того, небольшие собственные значения P(t) предполагают небольшуюдисперсию, что подразумевает низкую среднюю ошибку оценивания.
Поэтомунеобходимо выбрать такое L(t), чтобы минимизировать собственные значения P(t). Вспомним, чтоntr(P) = å l i ,i =1где tr(P) — след P, а лi являются собственными значениями P. Таким образом,минимизация tr(P) приводит к минимизации ковариации ошибок оценки.Фильтр Калмана получается нахождением такого L, которое бы минимизировало tr(P).В промежутке между измерениямиДифференцируя ~x, получим& = x& x&$ = Ax + Bu + о A x$ Bu = A ~x~x + о.Решая дифференциальное уравнение с начальными условиями ~x 0, получимr~x (t ) = e At x~0 + ò e A( t -t) x(t)dt.0166Глава 8. Оценка состоянияМожно рассчитать эволюцию ковариации ошибки P какd& ~ T + xx&T} =~~ T } = E { xx~~~P& = E { xxdt~~ T + x~x x T } = AP + PA T + E {x~x T } + E{ ~x x T }.= E { Axxx T + ~~x xA T + ~Также можно вычислить E{ ~xоT} какtt00E { x~x T } = E {e A( t ) x~0 x T (t )} + ò e A( t -t ) x(t)x T (t)dt = ò e A( t -t ) Qd(t - t)dt =1Q,2где 1/2 связана с тем, что используется половина площади внутри дельтафункции.
И т.к. Q симметрична, то P эволюционирует в промежутке междуизмерениями какP& = AP + P AТ + Q.При измеренииВо время измерения имеем~x + = x x$+ = x x$ L(Cx + з C x$) = x$ LC ~x Lз.Также можно записатьP + = E { x~ + x~ +T } = E {( x~ - - LCx~ - - Lh)( x~ - - LCx~ - - Lh) T } == E{ ~x-~x -T - ~x-~x - T C T LT - x~ - h T LT x - T + LCx~ - ~x - T C T LT + LCx~ - h T LT -LCx~ - ~x - T + Lh~x - T C T LT + Lhh T LT } =-Lh~(8.21)= P - - P - C T LT - LCP - + LCP - C T LT + LRLT ,где, поскольку з и ~x независимы, тоE {~x зТLТ} = E {Lз ~x Т} = 0.В последующем выводе потребуются следующие соотношения между матрицами:¶tr(BAD) = BТDТ,(8.22)¶A¶tr(ABAТ) = 2AB, если B = BТ.¶A(8.23)Наша цель — подобрать такое L, которое бы минимизировало tr(P+).
Необходимое условие этого имеет вид¶tr(P+) = PCТ PCТ + 2LCPCТ + 2LR = 0¶LÞ 2L(R + CPCТ) = 2PCТÞ L = PCТ(R + CP)1.8.5. Вывод дискретно-непрерывного фильтра Калмана167Подстановка в уравнение (8.21) даетP+ = P + PCТ(R + CPCТ)1CP PCТ(R + CPCТ)1CP ++ PCТ(R + CPCТ)1(CPCТ + R)(R + CPCТ)1CP == P PCТ(R + CPCТ)1CP = (I PCТ(R + CPCТ)1C)P = (I LC)P.Теперь можно кратко описать фильтр Калмана следующим образом.В промежутке между измерениями происходит распространение уравненийx&$ = A x$ + Bu,P& = AP + P AТ + Q,где x$ является оценкой состояния, а P — симметричная матрица ковариацииошибок оценки.
Когда получается измерение от i-го датчика, обновляетсяоценка состояния и ковариация ошибки в соответствии с уравнениямиLi = PCТi (Ri + Ci PCТi )1,P+ = (I LiCi)P,x$+ = x$ + Li (yi (tn) Ci x$),где Li носит название коэффициента усиления Калмана i-го датчика.Предположим, что модель распространения системы и модель измеренийлинейны. Однако для многих приложений, тех, которые будут описаны в этойглаве позже, модель распространения системы и модель измерений нелинейны. Другими словами, модель, представленная в (8.19), принимает видx& = f (x, u) + о,y[n] = h(x[n], u[n]) + з[n].Для этого случая распространение состояния и законы обновления используют нелинейную модель, но распространение и обновление ковариацииошибки используют Якобиан f для A и Якобиан h для C.
Результирующий алгоритм носит название расширенного фильтра Калмана (РФК). Псевдопрограмма для РФК представлена в алгоритме 2. Состояние инициализируется в1-й строке.Распространение обыкновенных дифференциальных уравнений (ОДУ) дляx$ и P, используя для этого схему интегрирования Эйлера, задается циклом спараметрами в строках 4—8. Уравнения обновления для i-го датчика приведены в строках 9—14.
Применение алгоритма 2 для оценки углов крена и тангажа описано в разделе 8.6. Применение алгоритма 2 для оценки положения, направления полета, скорости относительно земли, курса и скорости ветраописано в разделе 8.7.168Глава 8. Оценка состоянияАлгоритм 2. Дискретно-непрерывный расширенный фильтр Калмана1: Инициализировать: x$ = ч0.2: Выбрать частоту выборок выходных сигналов TВых., которая должна бытьменьше, чем частота выборок датчиков.3: В каждый момент времени дискретизации TВых.:4: for i = 1 до N выполнить {Шаг прогноза}æTö$ u)5: x$ = x$ + ç Вых.
÷ f ( x,è N ø¶f$ u)6: A = ( x,¶xæTö7: P = P + ç Вых. ÷ (AP + P AТ + Q)è N ø8: end for9: If Измерение было получено от i-го датчика, тогда {Обновление измерения}¶h$ u[n])10: Ci = i ( x,¶x11: Li = P C iT (Ri + Ci P C iT )112: P = (I LiCi)P$ u[n]))13: x$ = x$ + Li (yi [n] h( x,14: end if8.6. Îöåíêà ïîëîæåíèÿВ этом разделе описывается применение РФК для оценки углов крена и тангажа МБЛА. Для применения дискретно-непрерывного расширения фильтраКалмана, описанного в разделе 8.5, для оценок углов крена и тангажа используется нелинейная модель распространения& = p + q sin ц tan и + r cos ц tan и + оц,jq& = q cos ц r sin ц + ои,где в модель добавлены члены шума оц и ои, чтобы моделировать шум по p, q иr, где оц ~ N(0, Qц) и ои ~ N(0, Qи).Для получения выходных сигналов в качестве уравнений будут использоваться модели акселерометров. Из уравнения (7.1) получаем модель акселерометраöæ r& + qw - rv + g sin q÷ç(8.24)y уск.
= ç v& + ru - pw - g cos q sin j ÷ + h уск.ç w& + pv - qu - g cos q cos j ÷øè8.6. Оценка положения169& u, v и w. Сде& v,& w,Однако не существует метода для прямых измерений u,&лаем допущение, что u& = v& = w Ј 0. Из уравнения (2.7) имеемæ cos a cos b öæu ö÷çç ÷»vVa ç sin b÷.ç ÷ç sin a cos b ÷çw÷øèè øПредполагая, что a » q and b » 0 получимæ cos q öæu ö÷çç ÷÷.ç v ÷ » Va ç0ç sin q ÷çw÷øèè øПодставляя в уравнение (8.24), получимöæ qV a sin q + g sin q÷çy уск. = ç rV a cos q - pV a sin q - g cos q sin j ÷ + h уск.÷ç -qV cos q - g cos q cos jaøèОпределив x = (ц, и)Т, u = (p, q, r, Va)Т, о = (оц, ои)Т и з = (зц, зи)т, получимx& = f (x, u) + о,y = h(x, u) + з,гдеæ p + q sin j tan q + r cos j tan q ö÷÷,f ( x, u ) = ççøè q cos j - r sin jöæ qV a sin q + g sin q÷çh( x, u) = ç rV a cos q - pV a sin q - g cos q sin j ÷.÷ç -qV cos q - g cos q cos jaøèДля применения фильтра Калмана требуется Якобиан ¶f/¶x и ¶h/¶x.
Соответственно имеем¶f æç q cos j tan q - r sin j tan q=¶x ç-q sin j - r cos jèq sin j - r cos j ö÷,cos 2 q÷0øqV a cos q + g cos q0öæ¶h ç= - g cos j cos q -rV a sin q - pV a cos q + g sin j sin q ÷.÷¶x ç g sin j cos q(qV a + g cos j) sin qøèРасширенный фильтр Калмана применяется с использованием алгоритма 2.Для контрольного маневра, описанного в разделе 8.1, ошибка оценивания углов крена и тангажа при использовании алгоритма 2 показана на рис.
8.7.170Глава 8. Оценка состоянияf (град.)Оценивание угла кренаt (с)q (град.)Оценивание угла тангажаt (с)Рис. 8.7. Ошибки оценки по углам крена и тангажа, полученные с помощью расширенногодискретно-непрерывного фильтра Калмана: сплошная линия — измерения, пунктирная — оценкаСравнение рис. 8.7 с рис. 8.4 показывает, что дискретно-непрерывный расширенный фильтр Калмана дает лучшие результаты при полете с ускорением.8.7. Ñãëàæèâàíèå äàííûõ GPSВ этомжения,полетаоценкаразделе главы будут использоваться GPS-измерения для оценки полоскорости относительно земли, курса, скорости ветра и направленияМБЛА.
Если предположить, что угол траектории полета г = 0, тогдаположения дается соотношениямиp& n = Vg cos ч,p& e = Vg sin ч.Дифференцируя (7.21), получим, что оценка скорости относительно землиравнаdV& g =(V a cos y + w n ) 2 + (V a sin y + w e ) 2 =dt1& sin y + w& n ) +=[(V a cos y + w n )(V&a scos y - V a yVg& cos y + w& e )].+ (V a sin y + w e )(V&a sin y + V a yПредполагая, что скорость ветра и воздушная скорость постоянны, получим& sin y) + (V a sin y + w e )(V a y& cos y)(V cos y + w n )(-V a y.V& g = aVg8.7. Сглаживание данных GPS171Из уравнения (5.15) можно получить оценку ч:c& = g/Vg tanц cos(ч ш).Предполагая, что скорость ветра постоянна, получимw& n = 0,w& e = 0.Из уравнения (5.9) можно получить оценку ш, которая дается выражением& =qysin jcos j.+rcos qcos q(8.25)Определяя состояние как x = (pn, pe, Vg, ч, wn, we, ш)Т и входные данные какu = (Va, q, r, ц, и)Т, нелинейная модель распространения задается x& = f (x, u),гдеV g cos cöæ÷çVsincg÷ç& sin y) + (V a sin y + w e )(V a y& cos y) ÷ç (V a cos y + w n )(-V a y÷çVg÷çgf ( x, u) @ ç÷.tan j cos (c - y),÷çVg÷ç0÷ç0÷çsin jcos jq+r÷çøcos qcos qèЯкобиан f имеет видæ0çç0çç0¶f ç=¶x ç 0ççç0ç0è0где0 cos c -V g sin c00 sin c V g cos c0V& g& V a sin y0 0-yVg¶c&¶c&00¶V g¶c000000000000¶V& g¶y=00& V a cos yy0000& V a (w n cos y + w e sin y)-y,Vg¶c&g=tan j cos (c - y),¶V gV g200¶V& g¶y¶c&¶y000ö÷÷÷÷÷,÷÷÷÷÷ø172Глава 8.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
