導航與濾波書稿9附錄

A

V

VTVi特別地，若令V1V2V，則式（A-1）V(VV)(VV)V(VV

1 2(VV)V 其中，記矢量模值V

VV (VV)VV(VV) 假設u是與V同方向的單位矢量，即uV/u1。如果V是時變矢量，對模值Vu的VuVuVVu 考慮到固定長度的矢量及其變化率（矢端速度）之間是相互垂直的，即有uu0，因而式（A-4）可簡V 3，可得V(VV (VV V(VV) V(VV V

若對單位矢量u求導，并將式（A-6）ud(V/)VVVV(VV)/+VV(VV

另外，由式（A-7）中的uVV兩邊同時右叉乘uuuVVVVVVVV

(u)(u)(u)(u)(u)(u) 2(V)(V)(VV 2 2 1 2 1

B歐拉角的定存在32212種可能的定義方式。一般在給出歐拉角參數(shù)表示坐標系旋轉(zhuǎn)時，都得相應的歐拉角B-1B-2120o0o

z(z

z(z2(y300x1(x20yoyx1(x2) 0yoyx1(x2)B-1中，假設ox0y0z0為右手直角參考坐標系，對其實施如下三次轉(zhuǎn)動：首先ox0y0z0系繞oz0軸正向轉(zhuǎn)動角度得ox1y1z1系，顯然兩坐標系具有共同的oz軸；接著ox1y1z1系繞ox1軸正向轉(zhuǎn)動角度得ox2y2z2系，兩坐標系具有共同的ox軸；最后ox2y2z2系繞oz2軸正向轉(zhuǎn)動角度得ox3y3z3系，兩(+3)(+1)(+3步簡記為“313”，其中數(shù)字1，2和3分別表示繞oxoy和oz軸轉(zhuǎn)動，括號內(nèi)“+”號表示繞相應軸按右26坐標系ox3y3z3的方向余弦陣： 0 0 C0C0C1C2 s 12

0

（B- s

cssc

ss c cs

0sccc sscc

cs

s

s

其中，簡記三角函數(shù)ssin(ccos()(,ox0y0z0系至ox3y3z3

0

sC0C0C1C2

00

s

0 12

0c

s

s ccss

s csssc c cs 0sccs c sscsc

0c

c

可取向上為正。描述運載體的一組歐拉角通常也稱為姿態(tài)角，包括航向角（方位角或偏航角，ywazimuthhadigroll，B-3，詳細定義如下：航向角，運載體縱軸在當?shù)厮矫嫔系耐队熬€與當?shù)氐乩肀毕虻膴A角，常取北偏東為正，-90°～90°，或[π/2,π/2]橫滾角，運載體立軸與縱軸所在鉛垂面之間的夾角，當運載體向右傾斜時角度定義為正，角度范圍-180°～180°，或(ππ。 oxgygzg系和oxbybzb系，其中地理坐標系oxgygzg的三軸分別指向地理東向、北向和天向，俗稱“東-北-天”地理坐標系；運B-3給出的運載體歐拉角定義可以簡單描述為“(-3)12”方式。類似的，如果oxgygzg和oxbybzb歐拉角定義描述為“東-北-天(-3)12”或者“北-東-321”，含義就非常簡潔明確了。歐拉角、姿態(tài)陣和四元數(shù)之間的轉(zhuǎn)換關(guān)雖然航向角習慣上常定義為北偏東為正，但是當定義導航坐標系為“東-北-天”地理坐標系時，航(ππ2體坐標系（b系）的方向余弦矩陣CnCC

0 0

s

00 s 0

0c csssc

sccs c sscsc C 23

式中，C(i,j123表示矩陣Cn的第ij列元素，式（B-3）便是根據(jù)歐拉角（姿態(tài)角） b如果已知姿態(tài)陣Cn，通過觀察式（B-3，可得提取姿態(tài)角的數(shù)值方法如下所述b

arcsin(C32atan2(C,C

（B- atan2(C,C 其中，數(shù)值0.9999991atan2(yx為標準Ｃ語言函數(shù)庫xy不得同時為零，以atan2(C31,C33為例，它在Cn的第三行向量為單位向量且 0.999999時是可以保證C和C33不同時為零的 當C0.999999時，有π2，作近似sin1和cos0，則Cn

sCnscc sscc

0

atan2(C,C 當C0.999999時，有π2，作近似sin1和cos0，則Cn

c

sCnscc sscc

0

atan2(C,C 式（B-5）和式（B-6）顯示，當俯仰角在π2附近時，橫滾角和航向角之間是無法單獨分離0。arcsin(C atan2(C31,C33

atan2(C,C

atan2(C,C

23 q2

3q0q2q2q2q2q2

0.51C11C22 1C11C22q2q21C11C22

q

q2q2q2q2

q1C11C221C11C22q2q2q2q2 1C11C221C11C22

2(q1q2q0q3)

4q0q1C322(qqqq)

4qqC 1 0 0

q0 C21

4 4

2

q0q)

C13

2q3q0q1)

（B-10）0。由四元數(shù)歸一化條件q2q2q2q21可知，必然有max(q214q12 9）計算獲得某一個較大的元素qi（不妨取為正值（B-10）1C11C22在式（B-9）q10.51C11C22C330.5等價于1C11C22C331，即C11C221C11C22

q20.51C11C22C330.5等價于C22C11C33以及

C33C11C22 1q31n位四元數(shù)的含義式（2.4-21，在“東-北-312”歐拉角定義下，由歐拉角求解四元數(shù)的公式為b(c/2ks/)(c/(c/2c/2ic/2s/2(c/2c/2ic/2s/2c/2c/2c/2s/2c/2s/2c/2s/s/2s/2c/2c/

c/2/2/

127 arcsinatan22(qq 1

an22(q1q3b 歐拉角微分方假設姿態(tài)角，和均是時間的函數(shù)，對式（B-3） cssscCndsccs c sscsc dt (ccss)(sssccscsc scc (sscc

(cssc)(sscccccss cssscsccs c sscsc (scc (s

cc

(ccs (ccs ccs

0cs Cn

b

b

式（B-14）與方向余弦陣微分方程CnCn(ωb csωb ω

s

當c0時，對式（B-15） c cs

s

式（B-16）稱為歐拉運動學方程，由于分母中含c，在π/2附近無法通過角速度進行歐拉角的數(shù)值求解，因此，π/2是“東-北-312”歐拉角表示的奇異點。運載火箭上的歐拉角定π/2tttt，參見B-5。發(fā)射坐標系往往是當?shù)厮阶鴺讼?，其otxtotytot軸垂直于彈道平面向右，顯然otxtyt平面即為彈道平面。彈道平面（或otxt軸）與當?shù)氐乩肀毕虻膴A角A0。zbzbx1ybx2(xbozt(z1 圖B-5 如果運載火箭上裝有慣導系統(tǒng)（IMU，其軸向定義同樣參見圖B-5，當運載火箭水平“躺下”時，obxbybzb三軸分別為縱軸-立軸-橫軸，即“前-上-右”方向。按“321”方式定義歐拉角，其中俯仰角：火箭縱軸在彈道平面上的投影線與otxt軸的夾角，角度范圍為-180°～180°，或(ππ；偏航角：火箭縱軸與彈道平面的夾角，角度范圍為-90°～90°，或[π/2π/2]；滾動角：火箭立軸與縱軸所在鉛垂面的夾角，角度范圍-180°～180°，或(ππotxtytzt系至obxbybzb系的方向余弦陣為CtCC

0

0

0 00

ss s

0

s

c

c

sccs

s ccss csssc C 2317

0.999999

atan2(C21,arcsin(C

,C 可見，當π2時歐拉角表示正常，但π/2是運載火箭歐拉角表示方法的奇異點，這對于彈道羅德里格參一方面，對四元數(shù)表示法Q

cosusin Qsinusin

u2cos2 另一方面，由四元數(shù)微分方程Q1Qω2Q1cosusin

ω

T1

ω

2 2

u

2cos2

2

sin1sin usin1sin 2

u1uω1cotω

gf(

g的矢量方向即為轉(zhuǎn)軸方向u，其幅值是轉(zhuǎn)動角度大小f(時，廣義轉(zhuǎn)動矢量即為等效旋轉(zhuǎn)矢量g。24，可得

gf()uf

gf()(uTω)uf()1uω1cotω

2 25，可得gf()ωu(uω)f()1uω1cotu(u f()ω1f()uωf()1f()cotu(u

2f()ω1gω

f()1f()cotg(g

f2() 2 ω1ω11cot()2

2 2 在式（B-26）中，若取f()tan(/2)且記 g，則ξtan

uusin(/2)

cos(/ ξ1sec2ω1ξω

1 1 tan2(/2) 2 1tan21ω1ξω1ξ(ξ2 1ξTξ1ω1ξω1ξ(ξ 1ω1ξω1ξTξωξ(ξ 22 2式（B-29）最后一等號利用了公式（A-10，即(VTV)I(V)(V)VVT。在式（B-26）中，若取f() 2

σtan

uusin(/2)

1cos(/ 1σ1sec2ω1σω

1 1 tan2(/4) 2 1tan21ω1σω 1sec211tan2σ(σ4

tan2(/4) 4

4 1(σTσ1)ω1σω1σ(σ

1ω1σω1σTσωσ(σω)1 1ω1σω1σσTω1 41(1σTσ)I2(σ)2σσT4ξ為經(jīng)典羅德里格參數(shù)（Rodriguesparameters，而σ為修正羅德里格參數(shù)（modifiedparameters學方程式（B-29）描述剛體等效旋轉(zhuǎn)的最大連續(xù)轉(zhuǎn)角范圍為(π,π，不能進行全姿態(tài)描述；而若采用修正羅德里格參數(shù)σ，其奇異點為2π，對應最大連續(xù)轉(zhuǎn)角范圍為(2π,2π此外，在式（B-26）中，若取f()2tan(/2)且記 g，則類似于式（B-29）的推導，容易得lω1lω1ll 24lT24lT11ξT(111ξTQ 1

C姿態(tài)更新的畢卡算法、龍格—11多項式角運動描常值角速度（零次曲線假設在時間段[0,T內(nèi)，載體運動角速度ω(t

ω(t) (0tTtΔθ(t)0ω()dt

a為常數(shù)向量。若在采樣時間段[0,T內(nèi)進行一次角增量采樣，采樣時刻為T Δθ

線性角速度（一次曲線

aT

假設在時間段[0,T內(nèi)，載體運動角速度ω(t

ω(t)a

(0tT

Δθ(t)tω()dat0

a和b均為常數(shù)向量。若在采樣時間段[0,T內(nèi)進行兩次角增量采樣，采樣時刻分別為T/2和T， T

2T

TΔθ1

ω()da0

a 2

Δθ

Tω()dab2 Ta3T

T a3Δθ1

Δθ

b T拋物線角速度（二次曲線假設在時間段[0,T內(nèi)，載體運動角速度ω(t

ω(t)a2bt

(0tT

Δθ(t)tω()datbt20

ab和c均為常數(shù)向量。若在采樣時間段[0,T內(nèi)進行三次角增量采樣，采樣時刻分別為T/3，2T3和T

T

3T

T TΔθ1

ω()da

0

a3

b 2T

32T

Δθ2T

ω()da

T

a3

b 3

Δθ32T/3ω()da

a2T

b a11Δθ17Δθ2

Δθb

c9(Δθ12Δθ2Δθ3三次、四次曲線角速

ω(t)a2bt3ct2a25Δθ123Δθ213Δθ3

(0tT

b2(35Δθ169Δθ245Δθ311Δθ4

3Δθ

c 32(Δθ3Δθ3ΔθΔθd ω(t)a2bt3ct24dt3

0t

a137Δθ1163Δθ2137Δθ363Δθ4 25(45Δθ109Δθ105Δθ51Δθ10Δθb

7Δθ

c d625(3Δθ111Δθ215Δθ39Δθ42Δθ5 e625(Δθ14Δθ26Δθ34Δθ4Δθ5 abcd和e內(nèi)容詳見2.7.1節(jié)。顯然，前述零至四次曲線的擬合系數(shù)a, ,e也可以通過式（2.7-4）求取姿態(tài)更新的畢卡算11)21 1 T20)d11122T20) ω()1 ω(ω() ω()d 1T

2ω()

2300 TTI1ω(1)d1Δθ1

0TI 2(a2b)T

) T0(T2

2)d (ab2)T(a2b)(ab2)(a2b)T T

0

3aTb22bTb3)ab21(aTaT22aTbT3bTbT4)1abT 1(aTbT2)T(aTbT2)1abT 17I1Δθ24(ΔθΔθ

（C- ω(2)d2ω(3)dω(2)d2ω(3)d3T 00 (a2b2)d2(a2(a2b2)d2(a2b3)d3T00 T1(aTa22aTb3bTb4)1ab3

(a

)

3

（C-6

1(aTaT32aTbT47bTbT5)a(aTaT48aTbT5bTbT66

21ΔθTΔθ)Δθ(21ΔθTΔθ

5ΔθTΔθ)Δθ 在式（C-19）中，若作近似Δθ1Δθ2I

(20ΔθTΔθ)Δθ(20ΔθTΔθ)Δθ

1Δθ2Δθ1515161511I11IQ(0)11Δθ212

Q(T)

11

1I

11Δθ21Δθ2ΔθΔθ

2

42

2 2 Q(T)

11111 2

83

11

11Δθ21Δθ2ΔθΔθ Δθ2Δθ 2 2.5節(jié)等效旋轉(zhuǎn)矢量更新算法相比較，不難發(fā)現(xiàn)，基于線性角速度假設的二階（或三階）畢卡算法精姿態(tài)更新的四階龍格–庫塔算對于四元數(shù)微分方程Q(t)12

)

1KKω(T/K1Q(0)ω(T/ 2 K 1Q(0)K 2

1K2

ω(T/K1ω(T/2 2 Q(T)Q(0)6(K12K22K3K4假設在姿態(tài)更新周期[0,T內(nèi)角速度輸出為線性形式，根據(jù)式（C-5）和式（C-8）可得角速度與角增ω(0)a3Δθ1TT Δθω(T/2)abT

TTω(T)a2bT3Δθ2 即ω(t)a2bt3ct213a

(TtT

aΔθ17Δθ07Δθ1 bΔθ115Δθ015Δθ1

2(ΔθΔθΔθΔθc

Δθd T/2

0T其中Δθ1TT/2

ω()dΔθ0

T

ω()d為前一姿態(tài)更新周期的兩次角增量采樣，而Δθ1

ω()d和ΔθT/2

ω()d為當前更新周期的兩次角增量采樣。將式（C-25）代入式（C-ω(0)aΔθ17Δθ07Δθ1 T T T

Δθ5Δθ13Δθ

ω(T/2)a2b 4d

2 2 2 ω(T)a2bT3cT24dT33Δθ113Δθ023Δθ1 由數(shù)值計算原理知，RK4算法的單步截斷誤差為O(T5，這在圓錐運動環(huán)境下與基于等效旋轉(zhuǎn)矢量22Bortz方程ω1ω1()2ωRK4 K1Kω(T/2)

2 Kω(T/2)

(K)2ω(T/

TK ω(T/2)K

4

ω(T/2)

(K)2ω(T/22 T

K3 ω(T)K3

ω(T)

(K)2ω(T

6(K12K22K3K4(T2((T2(TTQ(T)

((T226姿態(tài)更新的精確數(shù)值為了書寫方面，以下記W(t)1ω(t，將四元數(shù)微分方程Q(t)1

ω(t) Q(t)Q(t)W

由數(shù)學知識知，任何連續(xù)函數(shù)都能用多項式以任意給定的精度近，這里假設W(t為時間t的有限階次N1。q(T,Q(Tq(T,T

q(T,0)10W(1)d1

T 32WT00 0N W()d

Tx

N2

T T

0

y0tN1

Wz1W W式中，記W(t)x

N2

;j0,123表示畢卡級數(shù)的第iW y 系數(shù)（下同

WzT2W(0 1xU(1)W1x

0TU(1)W0T 0U(1)W 0zU(1)W0z”表示兩個多項式系數(shù)行向量之間的卷積運算。T32W()00 1U(2)1 (2) 0U(2) 00數(shù)U(i1)僅僅是低一階系數(shù)U(i)與多項式系數(shù)W的卷積和，十分便于數(shù)值計算和軟件編程實現(xiàn)。 U(1)TN

U(2)T2N

U(3)T3N 0

0

0 U

TN

U T2N

U T3Nq(T,0)

1

U(2)

U(3) 2

2

2 tQ(t)Q(0)t7，將式Q(i1)(t)Q(0)tQ0

式中，右上角標“(i”表示迭代次數(shù)（i0,1

，可選迭代初值Q(0(Q(0。使用與式（C-同樣的計算方法不難由Q(i(t求得Q(i1(t，完成一次迭代，經(jīng)過多次（k次）迭代后，Q(k(t在T時刻的取值Q(k(T即為所需求解的姿態(tài)四元數(shù)Q(T2.7節(jié)介紹的等效旋轉(zhuǎn)m根據(jù)兩函數(shù)之積求導的二項式定理，對式（C-28）兩邊同時求mmQ(m1)(t)

CnQ(n)

W(mn)

m其中，Cnm

)28將Q(T在t02Q(T)Q(0)TQ(0)2

3Q(0)3

Q(0)

T

QQ

特別地，由于W(t)N1階的導數(shù)全為零，求解Q(m)(0N1N個求和項，Q(m1)(0)

CnQ(n)

W(mn) (0mN

m mCnQ(n)

W(mn) (mN行截斷近似即可求得Q(T的高精度數(shù)值解。最 姿態(tài)陣微分方程C(t)C(t)ω(t)與四元數(shù)微分方程Q(t)

W(tD假設有一右手直角坐標系obxbybzb（簡記為b系其坐標軸向單位矢量分別記為ibjb、kb；有一非直角坐標系oaxayaza（簡記a系其坐標軸向單位矢量分別記為iaja、ka。不妨設b系和a系具有共同的坐標原點，根據(jù)線性代數(shù)知識，從a系到b系的坐標變換矩陣可表示為ib

ib

ibka

pxzCbj j jk p b a yz

kb

kbka

pzz

11p2 pp1p1p2p2 yz

1p21p2 其中puvubva（uvxyz對應于uvijk）表示單位矢量ub在va上的投影大小，或者v在u上的投影大小。由于b系是直角坐標系，易知Cb的列向量必為單位向量，但其行向量 a一般不是單位向量，在矩陣Cb6a假設b系和a系對應軸向之間近似相互平行，或者說對應軸向之間的不平行偏差角為小量，即近似有ibiajbjakbka1，這時式（D-1）可近似為

pxzCb p

yzpuva以下分析Cb的矩陣分解及其幾何含義a正交三角分解（QR分解

1根據(jù)矩陣的QR分解理論，非奇異陣Cb總可以分解為單位正交陣Cb和上三角陣CB

BCbC B

在偏差角為小量情形下，式（D-2）表明Cb的對角線元素均為正且對角占優(yōu)，此處規(guī)定上三角陣CB的 μTBT在式（D-3）中，單位正交陣Cb可以看作是從b系到另一右手直角坐標系（B系）的坐標系μTBT陣，若記從b系到B系的失準角（即等效旋轉(zhuǎn)矢量）為μ

z且

BCbIB

1cos2

(

I(

a在式（D-3）中，上三角陣CB表示從非直角坐標系（a系）至直角坐標系（B系）的坐標變換矩陣，D-1所示。圖中，a系的oaxa軸與B系的oBxB軸重合；a系的oaya軸在B系的oBxByB平ja的端點在oBxB和oByBPxyPyya系的單位矢量kaaoBxBoByB和oBzB軸上的投影分別記為PxzPyzPzz。類似于式（D-1）iB

iB

iBka

CB

k

P P

B a yz

yz kB kBka

1

kB

iB(iaxB(xa

oB(oa

jaj

jByj Bay D-1ja繞kB旋轉(zhuǎn)zjB（jajB的有向角為zka在oByBzB平面上投影記為ka，兩者間夾角記為，矢量ka繞iB軸旋轉(zhuǎn)x角至kB（即從ka轉(zhuǎn)至kB的有向角為xka在oBzBxB平面上投影記為ka，兩者間夾角記為，矢量kBjB軸旋轉(zhuǎn)y角至ka（即從k轉(zhuǎn)至k的有向角為。若將φBay

cossiny 1

yCB0 cossin I

x x coscosx 1式中φ表示由矢量φ構(gòu)造的上三角矩陣0 yxφ x

0和ka在直角坐標系oBxByBzB坐標軸上的投影值；而式（D-6）中的元素x，y和z則表示從非直角坐標系的坐標軸oaya和oaza到直角坐標系所需轉(zhuǎn)動的偏差角，它們正好反映了非直角坐標系軸向之間的不正交程度，即x，yz分別表示oaya和oazaoaza和oaxaoaxa和oaya之間的不正交角，其值越小說a3a正交對稱分

Cb(Iμ)(Iφ)I(μ)

a根據(jù)矩陣的奇異值分解（SVD）理論，變換矩陣Cb非奇異，它總可以分解為如下形a CbUDVT(UVT)(VDVT)Cb

aU和VD是由CbBaCbUVTB系是直角坐標系；由于B系是直角坐標系，因而CBVDVT a量都是單位向量，又由于CB是對稱的，所以它的行向量也是單位向量aB與式（D-4）CbBBCbI(BT其中，μ 表示從b系到B系的失準角

a1的對稱陣CBa11

1 1 CB

I

x

122

1φ

yTTS(φ)

0

x 09aCb(Iμ)IS(φ)I(μ)a13，可得

pxz

z yy

p

yz

x

x

1

ypzx

z

pp,pp,p pzypyz

pxzpzx

pyx p

p

p

zy

zx

φ

yyy

zz

式（D-17）中的關(guān)系式φ2φ說明φ也具有不正交角含義。以zD-2，其幾何解釋是：逆著oBzB軸觀察，將oaxa軸和oaya軸同時投影到oBxByB平面上，分別記為oaxa和oaya，則有向角xBoBxayaoByBz，有向角的轉(zhuǎn)軸為oBzB軸正向；也就是說，逆著oBzB軸觀察，夾角xBoByB和xaoBya具有共同的對角線oBoD-2D-1中夾角xBoByB和oo(oBxaoo(oBoBxxoB(oa

zzz

z的幾何13立參數(shù)，只是各種參數(shù)的幾何含義不同罷了。順便，正交對稱分解方法給出的B系，它是所有右手直角坐標系中“最接近于”非直角坐標系aK。E時變系統(tǒng)的不可交換X(t)F(t)X(t)

式中，X(t)n維的狀態(tài)向量；F(t)，G(t)為確定性時變矩陣；u(t)為已知的控制輸入。根據(jù)線性系統(tǒng) X Φ(t,t0)X(t0 0Φ(tt0

Φ(t,t0)=F(t)Φ(t,t0狀態(tài)轉(zhuǎn)移陣Φ(tt0 tΦ(t,t0 F()0

1tF()1F1 窮重積分，當F(t)中元素是有界時，該級數(shù)總是收斂的，但通常得不到閉合解。狀態(tài)轉(zhuǎn)移陣Φ(tt0)具有傳遞性，即有Φ(t2,t0)=Φ(t2,t1)Φ(t1,t0)。但是，對于一般的高維時變系統(tǒng)而言，Φ(t2,t1)Φ(t1,t0Φ(t1t0)Φ(t2t1，這說明時變系統(tǒng)具有不可交換性，狀態(tài)轉(zhuǎn)移變化跟經(jīng)歷的路徑先特別地，對于定常系統(tǒng)，簡記F(t為F，則式（E-4） t0 F0=IF(t (t

=IF(t=eF(tt0 Φ(tt)=eF(t2t0)

連續(xù)時間系統(tǒng)的離散X(t)F(t)X(t)Z(t)H(t)X

1令離散化周期為Ts，采樣時刻為kTs(k1, 2，和tkX(t)Φ(t,

)X

)tkΦ(t,)G()u(

kk

k

k假設F(t、G(t和u(t在時間段[tk1tk]F(tk1、G(tk1和u(tk1F(t

TΦ(tk,tk

)

k

sIsF

k

)sF2

F

)(t

(t

Φ(tk,)

k1

I F(t

k

) F2(t

tk

(t (t k kΦ(tk,)G() k k

I F(t ) F2(t )

dG(tk1)u(tk1tk

tk1

tk 0

t Is

1!F(tk1)2!

(tk1)

dtG(tk1)u(tk1

TTIsF

T)sF2 )

k

k

k

k

T sF(tk1

(tk1)

)T k k k1XkΦk/k1Xk1Γk1uk

XkX(tkTΦk/kΓ

I

)sF22

k

uk1u(tk1注意，在多數(shù)文獻中常將離散輸入設置為

I k

k

k

由式（E-9）的矩陣指數(shù)表示可知，連續(xù)系統(tǒng)離散化后的狀態(tài)轉(zhuǎn)移矩陣Φkk1ZkHk

ZkZ(tk HkH(tkZXkΦk/k1Xk1Γk1ukZ

可控性與可觀

kHk對于離散時間系統(tǒng)式（E-14，它在時刻jN和有界輸入ui(ij,j1, ,jN1XjXjN0，這等價于如下定義的可控性矩陣行滿秩[rankC(j,jN)n]：C(j,jN)ΓjN1|ΦjN/jN1ΓjN2 |ΦjN/jΓj1或者等價于如下定義的格萊姆矩陣正定Λj,jN0ijN/ijNΛ(j,jN)C(j,jN)CT(j,jijN/ijN

系統(tǒng)（E-14）在時刻j完全可觀是指如果存在一個正整數(shù)N使得由量測Zi(ij,j ,jNXj H ΦjN/ΦjN/jj1/j

O(j,jN)HH

jj

ii/ ii/Θ(j,jN)OT(j,jN)O(j,jN)jNii/ ii/

XkΦXk1Γuk

ZkZ其中，ΦΓH都是常值矩陣。如系統(tǒng)式（E-19）完全可控則必定是一致完全可控的，系統(tǒng)完全可控

HΦ穩(wěn)定

HΦ

E-1給出了穩(wěn)定、漸進穩(wěn)定和不穩(wěn)定的示意圖。 圖E-1穩(wěn)定、漸進穩(wěn)定和不穩(wěn)定示意圖 XkΦk/k1Xk

ΦXe0處大范圍漸進穩(wěn)定的充要條件是：對于Bk0Ak0，滿足矩陣方程Φ

k/k

Bk

v(

,k)XTA 2（E-22）（間接法c10和c20kl0，2

cec1(tktl

式（E-25）蘊含的含釋如下：假設X1和X2為系統(tǒng)（E-22）的兩個不同初值，與它們對應的狀態(tài)

X 和X 和

X X X1X2 (X1X2

k X1X2

(X1X2)

(X1X2

c

(X1X2

k k nXkΦXk1，其漸進穩(wěn)定的充要條件是轉(zhuǎn)移矩陣Φn狀態(tài)觀測

XkΦXk1Γuk

ZkZHΦ其狀態(tài)觀測器結(jié)構(gòu)如圖E-3所示，圖中K稱為狀態(tài)觀測器的反饋矩陣，一般設計為定常矩陣，用于消除HΦΦHΓKΓ

XX

Z圖E-3 由圖E-3可得狀態(tài)觀測器的狀態(tài)方程為?k

K，若使得系數(shù)矩陣ΦKH的特征根都在單位圓內(nèi)，即便存在初始狀態(tài)估計誤差δX0X0?0，隨后的狀態(tài)估計誤差δXkXkX?k0，或者說，狀態(tài)觀測器的估計值X?k將漸進近系統(tǒng)狀態(tài)的真實值Xk。這說明，對于狀態(tài)觀測器式（E-29）(Φ

FKalman濾波公式的等價性證明。引理設(nmAA

A 22A11A22分別是n和m A1A1A(

A

)1A

A1A(

A

)1A1

11 2111 21 11 2111 2111 2111 21 2111

A

)1A

(AA

(

A

(

A

)1A 1222 1222 12 22 1222 22 1222 22 1222 1222

A

A1A1A(

A

)1A(

)1A1A1A(

A

)1A

2111 21A1A(

A

)1(

A

)1A

11 2111 1222 12A

0

A

I

AAA1A21

m

211112AA均非奇異，所以AA

2111

0 0n n

AAA21

Im

21

Im

A1A(

A

)1

11 2111

AA

(AAA1A 211112

2111

1

0nA1n

AAA1 A 21111221 m

A1A(

AA1

)1 0

11 2111

(AAA1A

A 2111 21 mA1A1A(

AA1

)1A

A1A(

AA1

)1 11 2111 21 11 2111 (

AA1

)1A

(AAA1

A

2111 21 2111 AA1AAA1 0A

1222

1222

AA

01 AA1A1 1222

1222

A22

(AA

0

AA1

1222

1222A1A(AAA1A 22 1222 22 (

A

(

A

)1A 1222 1222 12 22 22 2222 22 22 1222 12 22

A

A1A(

A

)1AA19，得證。M(AA

2111N(AAA1A

A1A1AMA

1222A1AM

NA A1

11 21 11

12

MAMA21

A1A A1A1ANA22 22 22 22 1222NAAAMA 11 21AAMAAMNA11 12(ABCD)1A1A1B(C1DA1B)1(ABCT)1A1A1B(ICTA1B)1CT

事實上，只要在式（F-3）AAABA1C

D，即可得式 15；而在式16AP1 AHT

AR

A

k

P(P1HT

k

k P

PHT(RHPHT)1H

(IKH

k

k1 kk

kk

kKPHT(RHPHT

k1 kk1K(P1HTR

)1HTR1PHT

k

GKalman遞推貝葉斯估

Xkf(

Zh(X,V 其中，Wk和Vk是已知概率密度函數(shù)的相互獨立的白噪聲過程；狀態(tài)方程等效描述了系統(tǒng)狀態(tài)轉(zhuǎn)移概率pxk|xk1pxk|x0x1,xk1)p(xk|xk1；量測方程等效描述p(zk|xk)。記某一樣本 列zkz1,z2 ,zk，貝葉斯估計的目的是由 列zk求解k時刻系統(tǒng)狀Xkpxk|zk10，可得p(

|z)p(zk|xk)p(xk)p(zk,zk1)|xkp(xk

p(z

p(z, kp(zk,zk1)|xkp(zk|xk,zk1|xkp(zk|xk)|(zk1|xk)p(zk1|xkpzk|(zk1,xk)p(zk1|xk

p(

|z)p(zk|xk)p(zk1|xk)p(xk p(zk,zk1 p(zk|xk)p(xk|zk1)p(zk1)/p(xk)p(xkp(zk|zk1)p(zk1p(zk|xk)p(xk|zk1p(zk|zk1p(xk|zk1)p(xk,xk1)|zk1dpxk|(xk1,zk1)p(xk1|zk1)dp(xk|xk1)p(xk1|zk1)dxkp(zk|zk1)p(zk,xk)|zk1dpzk|(xk,zk1)p(xk|zk1)dp(zk|xk)p(xk|zk1)d

其中，由于狀態(tài)轉(zhuǎn)移具有馬爾可夫性（狀態(tài)方程顯示Xk僅是Xk1Wk1的函數(shù)，從而有pxk|(xk1,zk1)p(xk|xk1)，p(xk|zk1)表示由量 列zk1獲得的關(guān)于系統(tǒng)狀態(tài)Xk的驗前概率密度函數(shù)；在量列zk給定后，p(zk|zk1)是一常數(shù)。注意：在上述式中省略了積分限，均為(,)，4p(

k|zk)

p(zk|xk)p(xk|zk1p(zk|xk)p(xk|zk1)d

和量測方程p(zk|xk]，實現(xiàn)了從k1pxk1|zk1到kpxk|zk的遞推求pxk|zk)計算條件均值即可獲得系統(tǒng)狀態(tài)的最小方差估計X?k,MV(zk)=EXk|zkxkp(xk|zk)d

Kalman濾波。Kalman濾波方程的推

XkΦk/k1Xk1Γk1Wk

ZHX 其中，Wk和VkEW k k kEV k kEWVTkjN(?VarXk1|zk1Pk1。根據(jù)式（G-9）中的狀態(tài)方程，考慮到Xk1與Wk1之間不相關(guān)，且Wk1zk1之間Xk的驗前分布均值和方差陣，分別為EXk|zk1E(Φk/k1Xk1Γk1Wk1)|zk1Φk/k1EXk1|zk1Γk1EWk1|zk1VarXk|zk1Var(Φk/k1Xk1Γk1Wk1)|zk1VarΦk/k1Xk1|zk1VarΓk1Wk1|zk1

P k/k

k

k/k

k

k

kEZk|xkHkVarZk|xk

4，考慮到式p(xk|zk)p(zk|xk)p(xk|zk1exp1

Hx)T

Hx)exp1(

)TP

(x

)（G- k kk k/k k/k k/k1 exp1(zHx)TR1(zHx)1(

)TP

(x k k k/k k/k k/k1 其中，符號“”表示“正比于。采用極大驗后估計法，在式（G-12）中l(wèi)np(xk|zk

0HT

Hx)

(x

)

k k k/k

k/k (P

HT

)1(P1

HTR1Zk

k/k

k k/k

k/k

k

HTH HTR)1 k/k1 kk/k1 Xk,MAPXkX?k,MAPXkX?k/k1Kk(ZkHkX?k/k1XkX?k/k1Kk(HkXkVkHkX?k/k1(IKkHk)(XkX?k/k1)由式（G-10）p(xk|zk1N(?k/k1Pk/k1，即式（G-15）

N(0Pk/k1，且從時序上易知(XkX?k/k1與量測噪聲Vk(IKH

(IKH)TKRK

k/k(IKkHk)Pk/k

kkH幾種矩陣分解方法（QR、Cholesky方根）UD（三角–對角）分解算法，下面分別予以介紹。QR分解（orthogonal- 矩陣的QR分解定理：設有列滿秩實矩陣AmnmnrankAmnn，則有矩陣分解 成立，其中QT 且 mn

mn

R

,R AT AiAi/

,, RAT AjAjRijAiA的第iRijR的第ijQmnARnnR

in,n R AT AiAi/

RAT AjAjRijCholesky三角分解（Cholesky根據(jù)矩陣理論，給定nP，它總可進行如下的三角分解（平方根分解P

PΔPPPP2nnPPnn且有PijPji(i,j12，得PP 1n002n2n0PP nn 0nnnnininPijijjji,j1j,j1i,j2j,j2

ij

kj

ik ijP

ij (P

k

ik)/

PP nkj200

kj

ik

(i(i(i

這便是求解平方根矩陣各元素的計算公式。不難發(fā)現(xiàn)，由PΔnn,n1,n,n2,n ,1,n11n

22,12

2n Δ

nnPP 00n12n0PP nn nn0n2nnijijPiji1j1i2j2i3j3

j1

(1in,1j

k

ik ij

(iP

i1

(i

k1j n1,n2,n3

(i即

21,22

31,32 ΔΔ000nnUD分解（unituppertriangular&diagonal給定nPPUDUP、上三角陣UD

U1n

0

U 0P

2n

U

2n

D

U D

nn

nn

nnPP U1nPP U1n00 02nUU2nD0U0PnP nn 0Unn0DnnUUnn（H-DnnU1n000D22D0nn2nU00DnnUnn5，式PijDjjUijUjjDj1,j1Ui,j1Uj,j1 DnnUinU

DUUDU

ij

k

kk jjij(P

DUU)/ kj

kk

(iUij (i (i

Djj

k

DkkD

上三角陣UDDnn,Un1,n,Un2,n ,U1,n即

D22,U12

U2nD/U

U1n PUD分解，即不一定存在上三角陣UDPUDUTPP0D0Uj1,j,Uj2,j ,U1,j0

8D 最后，三角分解和UD分解之間存在關(guān)系Δ D PUDUTU

DD)U(UD)(UD)

其中，DD的平方根矩陣，一般只需將DD的對角元素的正平方IEXm0且Cov(XmXnPmn（mni,jkl。特別地，當kl

P

j ik ij引理X~N(0PAB

（I-3）X是二維的，XX1

P

A

B

X P

B2 22

22 22P12P21A12A21B12B21。

X1X2

B12

X1X2=Etr

X X

tr

X X

22

2 22

22

2 22E(A11X1X1A12X2X1A21X1X2A22X2X2(B11X1X1B12X2X1B21X1X2B22X2X2

E

A21X1X2B11X1X1A21X1X2B12X2X1A21X1X2B21X1X2A21X1X2B22X2XA22X2X2B11X1X1A22X2X2B12X2X1A22X2X2B21X1X2A22X2X2B22X2X2lklk

2

EAXXBX

2

ABEXXXXj jij jiklllkjij jktr(ΑPBP)=tr

B12

P 22

22

22

22

APA A

A

B

B BPBP21 22 21 2222

21 22 21

22 APBPAPBPAPBPAPA

111111 111112 122111 122112l

k

j

l

k

j

tr(AP)tr(BP)=tr

B12

P 22

22

22

22 APBPAPBPAPBPAPB

122111 122112 122121 122122lklk

2

APB

2

2

ABPji1ijjikllkjijklij總結(jié)式（I-4）～ji1ijjikllkjijklijTnnll

k

ltr(ΑPBP)l

k

ll

k

j

ij其中，每個式子展開均含有n4AB。為敘述方便，將式（I-、式（I-8）和式（I-9）中的各項分別簡記為uijklvijklwijkl，按右下角標kij當kl A

(2P

PP

vijkkwijkk

ij j ij ik ij

該情況下，顯然滿足uijkk2vijkkwijkk當kluijklvijklwijkl

該情況下，單項雖不滿足uijkl2vijklwijkl，但是注意到以下兩項（交換下標kl）2(vijklvijlk)(wijklwijlk由此可見，無論下標i,jkl取何值，展開項均滿足式（I-

iJi命題若nAN個實數(shù)0

1(i1 N且N

1N1 1I I

x是nN

1xTx

xTNx

xxT1I i

i1x2Ni

x

Nii

1N2xi由于0i

1(i1 N且N

1，根據(jù)柯西不等式（見后，可得N1N20iii1Niii A 1

1

NN

NN 柯西不等式：設i，i(i1, iiiN2N2iii

NN1N2002N02N21且1（J-N2

i1

i1N2顯然，當iN2K三階方陣的奇異值分解（SingularValue position,SVD）在姿態(tài)陣的最優(yōu)估計中有著重要的應A，BATAB的特征值為0 稱i Ddiag(1

AUDV3U和V

HouseholderQR算法直接進行奇異值分解，數(shù)值精度高，只是過程稍顯復雜，計算量偏大。若先求解得矩陣平方BATAA的奇異值分再根據(jù)特征值逐一求解特征向量。因此，下面采用求根法直接求解實對稱陣BATA的特征值，再求解A的奇異值分解。該方法的優(yōu)點是計算量小。對稱矩陣Bf()det(IB)(B11B223a2ba(B11B22B33

b

(BBBBB2 1122 11Bij（i,j123）B的第ij列元素，且有BijBjix3

2x3pxq

p3ba2p3

2a39abqp3

q2

當0x1k1

1x

3i

kk1,2

2 1134q82

i2當03x 或xsign(3

3

pq0x1,2,30當0時，有三個互異實根x23pcos

3p

2p332p3

x3也必為實數(shù)，因此在上述求根公式中只有情形（2）和（3）矩陣B的特征值經(jīng)過大小排序之后，記為12