导航与滤波书稿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种可能的定义方式。一般在给出欧拉角参数表示坐标系旋转时，都得相应的欧拉角B-1B-2120o0o

z(z

z(z2(y300x1(x20yoyx1(x2) 0yoyx1(x2)B-1中，假设ox0y0z0为右手直角参考坐标系，对其实施如下三次转动：首先ox0y0z0系绕oz0轴正向转动角度得ox1y1z1系，显然两坐标系具有共同的oz轴；接着ox1y1z1系绕ox1轴正向转动角度得ox2y2z2系，两坐标系具有共同的ox轴；最后ox2y2z2系绕oz2轴正向转动角度得ox3y3z3系，两(+3)(+1)(+3步简记为“313”，其中数字1，2和3分别表示绕oxoy和oz轴转动，括号内“+”号表示绕相应轴按右26坐标系ox3y3z3的方向余弦阵： 0 0 C0C0C1C2 s 12

0

（B- s

cssc

ss c cs

0sccc sscc

cs

s

s

其中，简记三角函数ssin(ccos()(,ox0y0z0系至ox3y3z3

0

sC0C0C1C2

00

s

0 12

0c

s

s ccss

s csssc c cs 0sccs c sscsc

0c

c

可取向上为正。描述运载体的一组欧拉角通常也称为姿态角，包括航向角（方位角或偏航角，ywazimuthhadigroll，B-3，详细定义如下：航向角，运载体纵轴在当地水平面上的投影线与当地地理北向的夹角，常取北偏东为正，-90°～90°，或[π/2,π/2]横滚角，运载体立轴与纵轴所在铅垂面之间的夹角，当运载体向右倾斜时角度定义为正，角度范围-180°～180°，或(ππ。 oxgygzg系和oxbybzb系，其中地理坐标系oxgygzg的三轴分别指向地理东向、北向和天向，俗称“东-北-天”地理坐标系；运B-3给出的运载体欧拉角定义可以简单描述为“(-3)12”方式。类似的，如果oxgygzg和oxbybzb欧拉角定义描述为“东-北-天(-3)12”或者“北-东-321”，含义就非常简洁明确了。欧拉角、姿态阵和四元数之间的转换关虽然航向角习惯上常定义为北偏东为正，但是当定义导航坐标系为“东-北-天”地理坐标系时，航(ππ2体坐标系（b系）的方向余弦矩阵CnCC

0 0

s

00 s 0

0c csssc

sccs c sscsc C 23

式中，C(i,j123表示矩阵Cn的第ij列元素，式（B-3）便是根据欧拉角（姿态角） b如果已知姿态阵Cn，通过观察式（B-3，可得提取姿态角的数值方法如下所述b

arcsin(C32atan2(C,C

（B- atan2(C,C 其中，数值0.9999991atan2(yx为标准Ｃ语言函数库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。由四元数归一化条件q2q2q2q21可知，必然有max(q214q12 9）计算获得某一个较大的元素qi（不妨取为正值（B-10）1C11C22在式（B-9）q10.51C11C22C330.5等价于1C11C22C331，即C11C221C11C22

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

C33C11C22 1q31n位四元数的含义式（2.4-21，在“东-北-312”欧拉角定义下，由欧拉角求解四元数的公式为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 欧拉角微分方假设姿态角，和均是时间的函数，对式（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附近无法通过角速度进行欧拉角的数值求解，因此，π/2是“东-北-312”欧拉角表示的奇异点。运载火箭上的欧拉角定π/2tttt，参见B-5。发射坐标系往往是当地水平坐标系，其otxtotytot轴垂直于弹道平面向右，显然otxtyt平面即为弹道平面。弹道平面（或otxt轴）与当地地理北向的夹角A0。zbzbx1ybx2(xbozt(z1 图B-5 如果运载火箭上装有惯导系统（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是运载火箭欧拉角表示方法的奇异点，这对于弹道罗德里格参一方面，对四元数表示法Q

cosusin Qsinusin

u2cos2 另一方面，由四元数微分方程Q1Qω2Q1cosusin

ω

T1

ω

2 2

u

2cos2

2

sin1sin usin1sin 2

u1uω1cotω

gf(

g的矢量方向即为转轴方向u，其幅值是转动角度大小f(时，广义转动矢量即为等效旋转矢量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ξ为经典罗德里格参数（Rodriguesparameters，而σ为修正罗德里格参数（modifiedparameters学方程式（B-29）描述刚体等效旋转的最大连续转角范围为(π,π，不能进行全姿态描述；而若采用修正罗德里格参数σ，其奇异点为2π，对应最大连续转角范围为(2π,2π此外，在式（B-26）中，若取f()2tan(/2)且记 g，则类似于式（B-29）的推导，容易得lω1lω1ll 24lT24lT11ξT(111ξTQ 1

C姿态更新的毕卡算法、龙格—11多项式角运动描常值角速度（零次曲线假设在时间段[0,T内，载体运动角速度ω(t

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

a为常数向量。若在采样时间段[0,T内进行一次角增量采样，采样时刻为T Δθ

线性角速度（一次曲线

aT

假设在时间段[0,T内，载体运动角速度ω(t

ω(t)a

(0tT

Δθ(t)tω()dat0

a和b均为常数向量。若在采样时间段[0,T内进行两次角增量采样，采样时刻分别为T/2和T， T

2T

TΔθ1

ω()da0

a 2

Δθ

Tω()dab2 Ta3T

T a3Δθ1

Δθ

b T抛物线角速度（二次曲线假设在时间段[0,T内，载体运动角速度ω(t

ω(t)a2bt

(0tT

Δθ(t)tω()datbt20

ab和c均为常数向量。若在采样时间段[0,T内进行三次角增量采样，采样时刻分别为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内容详见2.7.1节。显然，前述零至四次曲线的拟合系数a, ,e也可以通过式（2.7-4）求取姿态更新的毕卡算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节等效旋转矢量更新算法相比较，不难发现，基于线性角速度假设的二阶（或三阶）毕卡算法精姿态更新的四阶龙格–库塔算对于四元数微分方程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假设在姿态更新周期[0,T内角速度输出为线性形式，根据式（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为前一姿态更新周期的两次角增量采样，而Δθ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 由数值计算原理知，RK4算法的单步截断误差为O(T5，这在圆锥运动环境下与基于等效旋转矢量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姿态更新的精确数值为了书写方面，以下记W(t)1ω(t，将四元数微分方程Q(t)1

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

由数学知识知，任何连续函数都能用多项式以任意给定的精度近，这里假设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表示毕卡级数的第iW y 系数（下同

WzT2W(0 1xU(1)W1x

0TU(1)W0T 0U(1)W 0zU(1)W0z”表示两个多项式系数行向量之间的卷积运算。T32W()00 1U(2)1 (2) 0U(2) 00数U(i1)仅仅是低一阶系数U(i)与多项式系数W的卷积和，十分便于数值计算和软件编程实现。 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”表示迭代次数（i0,1

，可选迭代初值Q(0(Q(0。使用与式（C-同样的计算方法不难由Q(i(t求得Q(i1(t，完成一次迭代，经过多次（k次）迭代后，Q(k(t在T时刻的取值Q(k(T即为所需求解的姿态四元数Q(T2.7节介绍的等效旋转m根据两函数之积求导的二项式定理，对式（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阶的导数全为零，求解Q(m)(0N1N个求和项，Q(m1)(0)

CnQ(n)

W(mn) (0mN

m mCnQ(n)

W(mn) (mN行截断近似即可求得Q(T的高精度数值解。最 姿态阵微分方程C(t)C(t)ω(t)与四元数微分方程Q(t)

W(tD假设有一右手直角坐标系obxbybzb（简记为b系其坐标轴向单位矢量分别记为ibjb、kb；有一非直角坐标系oaxayaza（简记a系其坐标轴向单位矢量分别记为iaja、ka。不妨设b系和a系具有共同的坐标原点，根据线性代数知识，从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根据矩阵的QR分解理论，非奇异阵Cb总可以分解为单位正交阵Cb和上三角阵CB

BCbC B

在偏差角为小量情形下，式（D-2）表明Cb的对角线元素均为正且对角占优，此处规定上三角阵CB的 μTBT在式（D-3）中，单位正交阵Cb可以看作是从b系到另一右手直角坐标系（B系）的坐标系μTBT阵，若记从b系到B系的失准角（即等效旋转矢量）为μ

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旋转zjB（jajB的有向角为zka在oByBzB平面上投影记为ka，两者间夹角记为，矢量ka绕iB轴旋转x角至kB（即从ka转至kB的有向角为xka在oBzBxB平面上投影记为ka，两者间夹角记为，矢量kBjB轴旋转y角至ka（即从k转至k的有向角为。若将φBay

cossiny 1

yCB0 cossin I

x x coscosx 1式中φ表示由矢量φ构造的上三角矩阵0 yxφ x

0和ka在直角坐标系oBxByBzB坐标轴上的投影值；而式（D-6）中的元素x，y和z则表示从非直角坐标系的坐标轴oaya和oaza到直角坐标系所需转动的偏差角，它们正好反映了非直角坐标系轴向之间的不正交程度，即x，yz分别表示oaya和oazaoaza和oaxaoaxa和oaya之间的不正交角，其值越小说a3a正交对称分

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

a根据矩阵的奇异值分解（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）中的关系式φ2φ说明φ也具有不正交角含义。以zD-2，其几何解释是：逆着oBzB轴观察，将oaxa轴和oaya轴同时投影到oBxByB平面上，分别记为oaxa和oaya，则有向角xBoBxayaoByBz，有向角的转轴为oBzB轴正向；也就是说，逆着oBzB轴观察，夹角xBoByB和xaoBya具有共同的对角线oBoD-2D-1中夹角xBoByB和oo(oBxaoo(oBoBxxoB(oa

zzz

z的几何13立参数，只是各种参数的几何含义不同罢了。顺便，正交对称分解方法给出的B系，它是所有右手直角坐标系中“最接近于”非直角坐标系aK。E时变系统的不可交换X(t)F(t)X(t)

式中，X(t)n维的状态向量；F(t)，G(t)为确定性时变矩阵；u(t)为已知的控制输入。根据线性系统 X Φ(t,t0)X(t0 0Φ(tt0

Φ(t,t0)=F(t)Φ(t,t0状态转移阵Φ(tt0 tΦ(t,t0 F()0

1tF()1F1 穷重积分，当F(t)中元素是有界时，该级数总是收敛的，但通常得不到闭合解。状态转移阵Φ(tt0)具有传递性，即有Φ(t2,t0)=Φ(t2,t1)Φ(t1,t0)。但是，对于一般的高维时变系统而言，Φ(t2,t1)Φ(t1,t0Φ(t1t0)Φ(t2t1，这说明时变系统具有不可交换性，状态转移变化跟经历的路径先特别地，对于定常系统，简记F(t为F，则式（E-4） t0 F0=IF(t (t

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

连续时间系统的离散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注意，在多数文献中常将离散输入设置为

I k

k

k

由式（E-9）的矩阵指数表示可知，连续系统离散化后的状态转移矩阵Φkk1ZkHk

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

可控性与可观

kHk对于离散时间系统式（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

系统（E-14）在时刻j完全可观是指如果存在一个正整数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都是常值矩阵。如系统式（E-19）完全可控则必定是一致完全可控的，系统完全可控

HΦ稳定

HΦ

E-1给出了稳定、渐进稳定和不稳定的示意图。 图E-1稳定、渐进稳定和不稳定示意图 XkΦk/k1Xk

ΦXe0处大范围渐进稳定的充要条件是：对于Bk0Ak0，满足矩阵方程Φ

k/k

Bk

v(

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

cec1(tktl

式（E-25）蕴含的含释如下：假设X1和X2为系统（E-22）的两个不同初值，与它们对应的状态

X 和X 和

X X X1X2 (X1X2

k X1X2

(X1X2)

(X1X2

c

(X1X2

k k nXkΦXk1，其渐进稳定的充要条件是转移矩阵Φn状态观测

XkΦXk1Γuk

ZkZHΦ其状态观测器结构如图E-3所示，图中K称为状态观测器的反馈矩阵，一般设计为定常矩阵，用于消除HΦΦHΓKΓ

XX

Z图E-3 由图E-3可得状态观测器的状态方程为ˆk

K，若使得系数矩阵ΦKH的特征根都在单位圆内，即便存在初始状态估计误差δX0X0ˆ0，随后的状态估计误差δXkXkXˆk0，或者说，状态观测器的估计值Xˆk将渐进近系统状态的真实值Xk。这说明，对于状态观测器式（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是已知概率密度函数的相互独立的白噪声过程；状态方程等效描述了系统状态转移概率pxk|xk1pxk|x0x1,xk1)p(xk|xk1；量测方程等效描述p(zk|xk)。记某一样本 列zkz1,z2 ,zk，贝叶斯估计的目的是由 列zk求解k时刻系统状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

其中，由于状态转移具有马尔可夫性（状态方程显示Xk仅是Xk1Wk1的函数，从而有pxk|(xk1,zk1)p(xk|xk1)，p(xk|zk1)表示由量 列zk1获得的关于系统状态Xk的验前概率密度函数；在量列zk给定后，p(zk|zk1)是一常数。注意：在上述式中省略了积分限，均为(,)，4p(

k|zk)

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

和量测方程p(zk|xk]，实现了从k1pxk1|zk1到kpxk|zk的递推求pxk|zk)计算条件均值即可获得系统状态的最小方差估计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。根据式（G-9）中的状态方程，考虑到Xk1与Wk1之间不相关，且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）中lnp(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根据矩阵理论，给定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

这便是求解平方根矩阵各元素的计算公式。不难发现，由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分解之间存在关系Δ 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总结式（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个实数0

1(i1 N且N

1N1 1I I

x是nN

1xTx

xTNx

xxT1I i

i1x2Ni

x

Nii

1N2xi由于0i

1(i1 N且N

1，根据柯西不等式（见后，可得N1N20iii1Niii A 1

1

NN

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

NN1N2002N02N21且1（J-N2

i1

i1N2显然，当iN2K三阶方阵的奇异值分解（SingularValue position,SVD）在姿态阵的最优估计中有着重要的应A，BATAB的特征值为0 称i Ddiag(1

AUDV3U和V

HouseholderQR算法直接进行奇异值分解，数值精度高，只是过程稍显复杂，计算量偏大。若先求解得矩阵平方BATAA的奇异值分再根据特征值逐一求解特征向量。因此，下面采用求根法直接求解实对称阵BATA的特征值，再求解A的奇异值分解。该方法的优点是计算量小。对称矩阵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也必为实数，因此在上述求根公式中只有情形（2）和（3）矩阵B的特征值经过大小排序之后，记为12