文件名称:
ArduPilot 中用到的 Rotation Vector in Attitude Estimation
开发工具:
文件大小: 298kb
下载次数: 0
上传时间: 2019-04-05
详细说明：ArduPilot 中用于EKF2公式推导的参考文档 Rotation Vector in Attitude EstimationPITTELKAU
Observe also that although a composition rule ( o 2 for
XI. Note that in both approaches the attitude perturbation is defined
otation vectors can be written(see Appendix), it is not globally by a composition, S=o(-)and dg=g q. therefore, the
nonsingular. However, q()=g(,o 2)=g(o,) q(2) and atlitude updale should be defined by a conposition Scbc d or 64 8 4
A( )=A(O,0)=A(1)A(?)can be regarded as nonsingular rather than an additive update. The estimation error for the rotation
equivalents. We will make use of this fact in choosing the lineariza
vectors, however, defined by S=sp-8. The a priori estimated
tion point for the bortz equation and also in formulating the attitude
So is zero and is updated additively by the EKF
propagation and update equations
Equation( 18)and the bias equation together in matrix form are
Linearized state equation
×]I[601,「mn
The bortz equation is augmented with the bias state to form the
00肠b
(19)
Sh
state equation
中4+中x+立中x(中x4)
where, to first order, the coeficient matrix is F()in Eq(14). The
discrete-time solution to this equation is
b
0
where w is given by Eq (8). Thc augmented statc cquation can be
Sxk k-1Sxk-1tTk-Iwk-I
(20)
written=f(r)
The approximated Bortz equation(11)must be linearized about where the state transition matrix k-1=pk-1(Ik)=p(tk, tk-1)is
some state trajectory i in order to build an EKE. Thus we seek the given b
linearized state equation x= F(r)ax, where
k-1()=4(-1+x,4-少「R(-1()S(中-1()
(14)
(21)
b
evaluated at T=Tk, where Tk=tr-tk-1. The rotation vector
a ab
pok-(t)is, to first order, the solution to Eq.(16). This approxima
tion is removed by letting k-I(t) be the solution (tk-1+t)to
We can consider the estimated body frame to be an instantaneousl
Eg (15)with initial condition( k-1)=0. Dcfinc=k-1(t)and
fixed inertial reference at the time of each measurement update p=l The matrices R and S are given by
(think of taking a snapshot of a moving frame). The Bortz equation
then describes the evolution of the attitude relative to this inertial
reference starting at ( k-1=0, where tk -i is the time of the mea
R()=(cos p)I
SIn pp
cos
[中×+
surement update
We seek a state transition matrix for the purpose of covariance
propagation. The simplest linearization is about =0, but with
some effort we can define a better linearization than that consider
S(d)=
sin p
cos yp
1×1+(g-s:ng
d
the referencetrajectory (t)given by the solution to the Bortz equa
tion for small rotations
(22b)
d
(15) The state transition matrix(2 1)is used only for propagatingthe EKF
variance according to
in the time interval E[k-1,t ], where (k-1)=0,w=w +b, is
the angular rate reference, and b is the bias reference The bias refer-
kk
P
1k-1k-1k-1
Q
ence trajectory b will be the a priori estimated bias, as is customary
in an eKF
where 2I- is the discrete-time process noise covariance(of
For the purpose of linearization, we can ignore the second-order Tk-1Wk-1). This is related to the continuous-time process noise
term中x(qx)inEq.(11)andφx(中×)inEq.(15, and so
by
the reference trajectory is approximately the solution to the coning
equation
k-1(1)Gk-1(1)(k-1(1)(k-1()Φk-1(1)d
q=+=φ
(16)
Q
The rotation error between the true rotation vector o from Eg (11
where Qk-1(1)=diag(Σa,Σ),Gk-1=l6,and
and the reference from Eq (15)is obtainedfrom the composition
rule for small rotations Eq(A4)
O
6=o(-)
φ-φ+bφ×φ
(17)
where o the variance of the angle white noise at the output of the
with So(t-1)=0. Differentiating this and simplifying(and omit
gyro. The discrete-time process noise covariance matrix is, to first
ting second-order terms)yields, with 8b=b-b,
6=-×8中+bb+a
8)
2+Ta2+
O
This copacetic result is equivalent to Eq. (135)of Ref. 1, Sec. XI,
Tall
where S=28g for the &q defined therein. In fact, this result can
he derived by using quaternions parameterized by the small-angle
rotation vectors and the procedureis the same as in Ref 1, Sec. where T= Tk
PITTELKAU
Filter State Propagation
Filter State Update
In general, w is not colinear with q because its direction can vary
The rot ation vector estimate and bias perturbation estimate are
with time. This is especially true of highly maneuverablespacecraft
updated according to
Coning motion will reduce the accuracy of the attitude estimate un
less the attitude reference is propagated at a very high rate. This is
中k
usually accomplished by propagating the attitude quaternion with
84k-1+Kk
(30)
each gyro sample, which is inefficient if it is performed at a high
rate. Savage gives general algorithms for numerically integrating
where
the bortz equation for attitude propagation using the gyro mea-
SureInents. We present one of the simplest of such algorithIns to
propagate the reference state equation (15)
(31)
In this algorithm the gyro sampling and the attitude propagation
8b
are performed at a fast rate and the attitude measurement and filter
update are performed at a slow rate. Let T be the fast cycle interval
is the a priori estimate of the rotation vector relative to the attitude
Ts the slow cycle interval, and m the number of fast cycles per slow
frame represented by gulk-I. We also have the Kalman gain matrix
cycle such that Tf=Ts/m. Let Ok.e be the gyro measurement al
Kk=Pkk-1Hk(Hk|Hk +), where Rk is the measure
time tk +(e-1)Tf and OK,0=0k-Im. The following propagation
ment covariance. The residual vk and the measurement sensitivit
algorithm from Ref. 11, Eqs.(46)and(47), is one of the simplest
matrix Hk correspond to one of the measurement types defined in a
later section
and most efficient that can account for coning. For 1o:(frame r-frame b), we can use the first-order
cient to compute the measurement sensitivity matrix
approximation A()ci-[ox], and so we have
H
(38)
v=TSA(o5)
The derived measurement meas is easily and accurately computed
from the quaternion quotient
TA()A(: )v
q(φm)=q8(G8)
TA(φ)
(39)
cT,(I-[φ×)y
where q" is the quaternion measurement at time k(which represents
the attitude of the measured sensor frame m), s is the body-to
=Tbv+Tb×|φ
(45
sensor trans fornation quaternion, and 4=9k1k-1 is the a priories
timated attitude quaternion given by Eq (28a). Note that the derived
noting that the r frame coincides with the b frame when =0, we
measurement meas is also the residual vr(also called the predic
obtain
tion error or innovations )because the a priori estimate of s p is zero
(The a priori estimate is zero unless multiple updates are performed
dy
without moving the estimate to nonsingular storage for efficiency
=T[v3×]
reasons, in which case the residual is simply vk =dmeas -S.
φ=0
If the filter is initialized such that H, P1oHi > Rl, then
So(ts)meas so that after moving the attitude information into
The measurement sensitivity matrix for the vector sensor is then
the quaternion we get
/n0-v/(v)
q=q(中)②q1
2)
T5p”×](47)
g()m)8q0
01/n2-2/(
(40)
where the vectors v'=(Ur, Us, Us)and y" are computed using the
a priori estimate of thc attitude quaternion. The prcdictcd measure
where the difference between the m frame and the s frame is the
ment is given by
measurement noise. This filter is well behaved because the esti-
mated quaternion equals the measured quaternion converted to the
=h(G)
body frame when the initial reference attitude is far from the first
Measured attitude and the initial covariance is very large (ideally
where
infinite). In general, nonlinear filters should be initialized with an
estimate near the true state to avoid possible divergence problems
讠=TA(,)v
(49)
For vector sensors which do not observe attitude about their line of
determination filter as described in the Initialization section ttitude
sight, it is a matter of good general practice to initialize the
andA(D=A,
the a prioriestimated attitude from eg. (28b)
(Note that vh can be computed directly IrOIn qEIk-1 So that Akk-I
also have to be computed. The residual is simply
Sensitivity matrix: Focal Plane measurements
.=
Consider a vector v=lu r, us, u in the sensor reference
frame s that is measured by some generally nonlinear function with
Sensitivity Matrix: Vector Measurements
additive noise e
The measurement function for a three-axis magnetometer is
y=h(w)+∈
y=ν+
For a focal plane measurement
Because the three-axis magnetometer is a true vector sensor all
three axes are measured, and so ah/ a(v)=I.The measurement
sensitivity matrix IS
y
(42)
(51)
The predicted measurement is given by
We nccd to compute the measurement sensitivity matrix
(52)
dh dy
H
ch(v)
pa()70
(43)
and the residual is simply Dk=yr-yx
860
PITTELKAU
Initialization
For small I and small p2, we can make the approximation
A correct implementation of any nonlinear filter, including an
attitude determination filter-correct in the sense of good design
φ=φ1+中2-中1X中
(A4)
practice and numerically robust-would initialize the reference at-
titude with a quaternion measurement or a quaternion derived from
Based on this result, a first-order attitude propagation(integration)
vector measurements so that the initial attitude error is no greater
algorithm can be written for rate-integrating gyros. It is only first
order because the rotation vector is not equal to the integral of the
than the measurement error. This avoids linearization problems and
concomitant convergence problems. The attitude covariance is ini-
angular rate in the presence of coning motion. For small angula
tialized to the measurement error covariance. The measurement up
increments this first-order algorithm is equivalent to accumulating
dale is then nowhere near a singularity, and subsequent prediction
the angular increments with the computationally more deManding
errors and state updates are small. A large initial bias error is of no
quaternion product q(中)=q(1)8q(中2)
consequences long as the bias covariances initializedaccordingly
Acknowledgment
Conclusions
The idea for this paper arose during a discussion with Landis
Markley of the nasa goddard Space Flight Center at a conference
The differential equation for the evolution of the rotation vector,
known as the Bortz equation, was introduced as a kinematic model
in 2001 where I ex pressed concern that some liberties apparently
for attitude determination. Although the bortz equation exhibits a
had been taken in the development of the attitude determination
filter in Ref. 1. I thank Landis for his interest in this paper
singularity, as do all three-dimensional attitude parameterizations
singularity is avoided by storing the attitude informationin a quater
References
nion or direction cosine matrix (DCM)and by maintaining the rota
tion vector near zero. The quaternionor DCM itselfis not an intrinsic
Lefferts, E.J., Markley, F. L, and Shuster, M.D., Kalman Filtering for
part of the design of the filter-the attitude information couldjust as
Spacecraft Attitude Estimation "Journal Guidance, ControL, and Dynam-
easily have been stored in any nonsingularattitude representation
ics,Vol.5,No.5,1982,pp.417-429
2Kau, S, Kumar, K, and Greenly, G, "Attitude Determination via Non-
and so technicalissues associated with otherapproachesdo not arise
linear Filtering, IEFE Tran.sactions on Aerospace and Elect mnic Systems
in the derivation in this paper
Vol.AES-5,Nov.1969,p0.906-911
In cffcct the filter docs not know any thing about the attitude
3Bar-Itzhack,L.Y. and Idan.M. "Recursive Attitude Determination from
Quaternion or DCM. It knows only that before each update it starts
Vector Observations: Euler Angle Estimation, Journal of guidance, Con
at zero rotation with an a priori covariance of attitude error. con-
trol, and DynamicS, Vol. 10, No. 2, 1987, pp. 152-157
ceptually, this means that the attitude quaternion or DCm carries
Idan, M, "Estimation of Rodrigues Parameters from Vector Observa
no error and the covariance matrix represents the error in the fil
tions, IEEE Transactions on Aerospace and Electronic Systems, Vol. AES
ter's estimate of attitude, where the filter's estimate always has zero
32,No.2,1996,pp.578-586
Crassidis, J. L, and Markley, F L, "Attitude Estimation Using Modified
mean but represents attitude relativeto the quaternionor dCM. said
Rodrigues Parameters. Flight Mechanics and Estimation Theory Sympo
another way, once it is computed, the attitude quaternion or Do
imn, NASA CH333,1995,p.7183.
is simply a perfectly known and convenientinertial reference frame
6 Oshman, Y, and Markley, F. L, "Minimal-Parameter Attitude Matrix
for the filter
Estimation from Vector Observations, Journal of Guidance, Control, and
Various nunerical integration algorithms for the Bortz equation
Dynamics,Vol.21,No.2,1998,pp.595-602
are available that account for coning, which occurs when the an
OShman, Y, and Markley, F.L., "Sequential Attitude and Attitude -Rate
gular rate vector is not parallel to the rotation axis. Coning can be
Estimation Using Intcgratcd-Ratc Paramctcrs, Journal of Guidance, Con
caused by certain spacecraftmaneuvers or by vibration (jitter). Effi
trol, and Dynamics, Vol 22, No 3, 1999, pp. 385-394
Deutschmann, J, Markley, F. L, and Bar-Itzhack, I, Quaternion Nor-
cient low-order algorithms that effectively account for coning were
malization in Spacecraft Attitude determination, Proceedings of the AlAa
introduced
Guidance, Navigation, and Control Conference, AlAA, Washington, dC,
1991,p.908-916
ppendix: Composition Rule for rotation vectors
Bar-Itzhack, I, Deutschmann, J, Markley, F. L, and"Quaternion Nor-
A compositionruleφ=φ1oφ2 for two rotation vectors1and2
malization in Spacecraft Attitude determination, Flight Mechanics and
Estimation Theory Symposium, 1992, pp 441-454
is easily derived froin the quaternion producty b=g(o,(2)
Bortz,I. E, " A New Mathematical Formulation for Strapdown Iner
tial Navigation, IEEE Transactions on Aerospace and Electronic Systems,
1,sin(g/2)
Vol.AES-7,No.1,1971,PP.6166
I Savage, P. G, ""Strapdown Inertial Navigation Integration Algorithm
q(o)
(Al) Design Part 1: Attitude Algorithms, Journal of Guidance, Control, and
cos(φ/2)
Dynamics, Vol 21, No 1, 1998
2Shuster, M. D, "The Kinematic Equation of the Rotation Vector, "IEEE
Transactions on Aerospace and Electronic Systems, vol. AES-29, No. I
where p=loI and similarly for q(o1) and q(2). The quaternion 1993, pp. 263-26
product is defined b
13 Markley, F. L, "Attitude Error Representations for Kalman Filter
ing Journal of Guidance, Control, and Dynamics, Vol. 26, No 2, 2003
1I-[r1×]
pp.311-317
q(中1)②q(2)=
Pittelkau, M. E,"Kalman Filtering for Spacecraft Syste Alignment
s1」LS2
Calibration, Jounal of guidance, Control, and Dynamics, Vol 24, No 6
2001,pp.1187-1195
From these definitions it is easily shown that
15Pittclkau, M.E., "Evcrything is Rclativc in Systcm Alignment Calibra-
tion, Journalof spacecraft and Rockets, Vol 39, No. 3, 2002, pp. 460-466
sin(/2)
16 Stuelpnagel, J, "On the Paramerization of the Three-Dimensional Ro-
y/2
p/2s(212)+,Sin(oDos(q1/2)
y2/2
0 Wertz, J.R(ed. Spacecraft Attitude Determination and Control.
ional Group, "SIAM Review, Vol 6, No 4, 1964, pp. 422430
1x,s(q/2)si(g/2)
Kluwer Academic, Boston, 1978, p. 565
(A3)
18 Shuster, M. D, "A Survey of Attitude Representations, "Journal ofthe
Astronautical Sciences, Vol 41, No 4, 1993, pp. 439-517
from which the composition rule can be written. The coefficient
19Ronen. M. and Oshman.Y."A Third-Order Minimal-Parameter Solu
of p on the left renders undefined when the angle of rotation
tion of the Orthogonal Matrix Differential Equation, Journalof guidance,
Dynamics,ol.20,No.3,1997,p.516521
op reaches a nonzero multiple of 2I. This is a consequence of the
Bierman, G. J, Factorization Methods for Discrete Sequential estima
fact that there is no three-parameter representation of attitude that tion, Vol. 128. Mathematics in Science and Engineering, Academic Press
is globally nonsingular
New York. 1981
《知网查重限时7折最高可优惠120元「立即检测
本科定稿,硕博定稿,查重结果与学校一致
免费论文查重:htp:/www.paperyy.com
3亿免费文献下载:htp:/www.ixueshu.com
超值论文自动降重:htp:www.paperyy.com/reduce_repetition
PPT免费模版卜载:htp:pptxushu.com
阅读此文的还阅读了:
1. Attitude Coordination Control of Spacecraft Formation Flying Using Rotation Matrix
2. RESEARCH OF SENSORLESS VECTOR CONTROL SYSTEM AND SPEED ESTIMATION
Shield Attitude Rectification Decision Based on Support Vector Data Description
4. Particle Filter Based Attitude Estimation with Inertial Measurement Units
5. 6-DOF MOTION AND CENTER OF ROTATION ESTIMATION BASED ON STEREO VISION
6. Sine rotation vector Method for Attitude estimation of an Underwater robot
7. Adaptive robust cubature Ka.man filtering for satellite attitude estimation
8.一种改进的旋转欠量姿态算法
9. An Optimal Estimation Method for Multi-Velocity Vector Integration in Spacecraft Celestial Navigation
10. Aircraft Attitude estimation bascd on central differencc kalman filter
11.A New Rotation Estimation and Recovery Algorithm
12. SUPPORT VECTOR MACHINE APPROACH TO DRAG COEFFICIENT OESTIMATION
13. Marginalized particle filter for spacecraft attitude estimation from vector measurements
14.Application of the sun line-of-sight vector in the optimal attitude estimation of deep-space probe
OA estimation for attitude determination on communication satellites
16. Quadrotor Aircraft Attitude Estimation and Control Based on Kalman Filter
17.一种单子样旋转矢量姿态算法
18.Application of the sun linc-of-sight vector in the optimal attitude estimation of decp-space probe
19.IEEE 2013 Fourth International Conference on Intelligent Control and Information Processing (ICICIP)-Beijing, China(201
20.6-DOF MOTION AND CENTER OF ROTATION ESTIMATION BASED ON STEREO VISION
21. Distributed High-Gain Attitude Synchronization Using Rotation Vectors
22.三周期旋转矢量姿态算法研究
23. Distributed High-Gain Attitude Synchronization Using Rotation Vectors
24. Estimation of the Attitude in an Industrial Robot: An Unit Quaternion Approach
25. Multi-invariance MUSIC Algorithm for DOA Estimation in Acoustic Vector-Sensor Array
26. Iterated Unscented Kalman Filter for Spacecraft Attitude Estimation
27. Rotation Vector in Attitude estimation
28 Rotation vector in Attitude estimation
29. Attitude Estimation of 3-DOF Lab Helicopter Based on Optical Flow
30 Distributed High-Gain Attitude Synchronization Using Rotation Vectors
31 On Optimum Data Vector for InSAR Interferometric Phase Estimation
32. Estimation of airspeed based on acoustic vector sensor al
33 Propagator Method Based Two-Dimensional DO A Estimation for Acoustic Vector-Sensor Array
34 2-D DOA and Polarization Estimation of LFM signals with One Electromagnetic Vector Sensor
35. Rotation-invariant scene matching with multi-sample parallel estimation in volume holographic correlator
36. Attitude Determination System Based on Rotation Mechanism and Beidou Receivers
37. A method for improving the precision of motion vector estimation based on gyroscope
38.A UAV Aided Method for Ship Attitude Estimation
39. ESTIMATION OF 2-D DOA JOINT FREQUENCY OF SIGNAL VIA A SINGLE VECTOR HYDROPHONE
40 Feasibility analysis for attitude estimation based on pulsar polarization measurement
41. Parameter Estimation of Distributed Sources with Electromagnetic Vector Sensors
42.等效旋转矢量法在旋转弹姿态解算中的应用
43 In-flight Misalignment Attitude Estimation for UAV Based on GPS and UKF/MPF Filter
44 DOA estimation for attitude determination on communication satellites
45.扩展旋转欠量捷联姿态算法
46 Rotation Invariant Texture Classification with Dominant Orientation Estimation Based on Gabor Filters
47.旋转矢量航姿算法的一种新的表达式
48. Color Optical Flow Estimation Based on Vector Homogeneity Analysis
49. Venture capital project estimation based on cost sensitive support vector machines
0.MVM-DCT域的运动矢量估计
(系统自动生成,下载前可以参看下载内容)
下载文件列表
相关说明
- 本站资源为会员上传分享交流与学习,如有侵犯您的权益,请联系我们删除.
- 本站是交换下载平台，提供交流渠道，下载内容来自于网络，除下载问题外，其它问题请自行百度。
- 本站已设置防盗链，请勿用迅雷、QQ旋风等多线程下载软件下载资源，下载后用WinRAR最新版进行解压.
- 如果您发现内容无法下载，请稍后再次尝试；或者到消费记录里找到下载记录反馈给我们.
- 下载后发现下载的内容跟说明不相乎，请到消费记录里找到下载记录反馈给我们，经确认后退回积分.
- 如下载前有疑问，可以通过点击"提供者"的名字，查看对方的联系方式，联系对方咨询.
