1909 lines
56 KiB
Plaintext
1909 lines
56 KiB
Plaintext
Beacon Position and Attitude Navigation Aided by a Magnetometer
|
||
by Xu Ma and Gonzalo R. Arce
|
||
|
||
ARL-CR-650
|
||
|
||
June 2010
|
||
|
||
prepared by
|
||
University of Delaware Department of Electrical and Computer Engineering
|
||
140 Evans Hall Newark, DE 19716
|
||
under contract
|
||
W911QX-07-C-0053
|
||
|
||
Approved for public release; distribution is unlimited.
|
||
|
||
NOTICES
|
||
Disclaimers
|
||
The findings in this report are not to be construed as an official Department of the Army position unless so designated by other authorized documents.
|
||
Citation of manufacturer’s or trade names does not constitute an official endorsement or approval of the use thereof.
|
||
Destroy this report when it is no longer needed. Do not return it to the originator.
|
||
|
||
Army Research Laboratory
|
||
Aberdeen Proving Ground, MD 21005-5066
|
||
ARL-CR-650
|
||
|
||
June 2010
|
||
|
||
Beacon Position and Attitude Navigation Aided by a Magnetometer
|
||
Xu Ma and Gonzalo R. Arce University of Delaware
|
||
|
||
prepared by
|
||
University of Delaware Department of Electrical and Computer Engineering
|
||
140 Evans Hall Newark, DE 19716
|
||
under contract
|
||
W911QX-07-C-0053
|
||
|
||
Approved for public release; distribution is unlimited.
|
||
|
||
REPORT DOCUMENTATION PAGE
|
||
|
||
Form Approved OMB No. 0704-0188
|
||
|
||
Public reporting burden for this collection of information is estimated to average 1 hour per response, including the time for reviewing instructions, searching existing data sources, gathering and maintaining the data needed, and completing and reviewing the collection information. Send comments regarding this burden estimate or any other aspect of this collection of information, including suggestions for reducing the burden, to Department of Defense, Washington Headquarters Services, Directorate for Information Operations and Reports (0704-0188), 1215 Jefferson Davis Highway, Suite 1204, Arlington, VA 22202-4302. Respondents should be aware that notwithstanding any other provision of law, no person shall be subject to any penalty for failing to comply with a collection of information if it does not display a currently valid OMB control number.
|
||
PLEASE DO NOT RETURN YOUR FORM TO THE ABOVE ADDRESS.
|
||
|
||
1. REPORT DATE (DD-MM-YYYY)
|
||
June 2010
|
||
|
||
2. REPORT TYPE
|
||
Final
|
||
|
||
3. DATES COVERED (From - To)
|
||
September 2007–August 2009
|
||
|
||
4. TITLE AND SUBTITLE
|
||
Beacon Position and Attitude Navigation Aided by a Magnetometer
|
||
|
||
5a. CONTRACT NUMBER
|
||
W911QX-07-C-0053
|
||
|
||
5b. GRANT NUMBER
|
||
|
||
5c. PROGRAM ELEMENT NUMBER
|
||
|
||
6. AUTHOR(S)
|
||
Xu Ma and Gonzalo R. Arce
|
||
|
||
5d. PROJECT NUMBER
|
||
08B3K1
|
||
5e. TASK NUMBER
|
||
|
||
5f. WORK UNIT NUMBER
|
||
|
||
7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES)
|
||
University of Delaware Department of Electrical and Computer Engineering 140 Evans Hall Newark, DE 19716
|
||
9. SPONSORING/MONITORING AGENCY NAME(S) AND ADDRESS(ES)
|
||
U.S. Army Research Laboratory RDRL-WML-F Aberdeen Proving Ground, MD 21005-5066
|
||
|
||
8. PERFORMING ORGANIZATION REPORT NUMBER
|
||
10. SPONSOR/MONITOR’S ACRONYM(S)
|
||
ARL-CR-650
|
||
11. SPONSOR/MONITOR'S REPORT NUMBER(S)
|
||
|
||
12. DISTRIBUTION/AVAILABILITY STATEMENT
|
||
Approved for public release; distribution is unlimited.
|
||
|
||
13. SUPPLEMENTARY NOTES
|
||
|
||
14. ABSTRACT
|
||
Position and attitude estimations are often prerequisites for numerous applications, such as munition systems, mobile robots, and autonomous vehicles. In this report, a position and attitude parameter estimation algorithm suitable for projectiles and microaerial vehicles is developed. The algorithm uses the Earth’s magnetic field and a set of fixed beacons as observation vectors. The magnetic field vector observation is obtained from a magnetometer equipped on the object. In addition, a set of angle of arrival vectors is obtained from N-emitting beacons at known ground positions. The algorithm uses iterative, nonlinear object state estimation based on observation vectors to estimate the position and attitude. Position is estimated using the resection algorithm, and attitude is estimated using a two-dimensional direction of arrival estimation algorithm. The proposed method is demonstrated to provide a full position and attitude solution while satisfying the stringent requirements of low-cost and real-time estimation suitable for gun-launched projectiles.
|
||
15. SUBJECT TERMS
|
||
navigation, attitude, position, estimation, DOA, AOA
|
||
|
||
16. SECURITY CLASSIFICATION OF:
|
||
|
||
a. REPORT
|
||
Unclassified
|
||
|
||
b. ABSTRACT
|
||
Unclassified
|
||
|
||
c. THIS PAGE
|
||
Unclassified
|
||
|
||
17. LIMITATION 18. NUMBER
|
||
|
||
OF ABSTRACT
|
||
|
||
OF PAGES
|
||
|
||
UU
|
||
|
||
38
|
||
|
||
19a. NAME OF RESPONSIBLE PERSON
|
||
Gary L. Katulka
|
||
19b. TELEPHONE NUMBER (Include area code)
|
||
410-306-1071
|
||
|
||
Standard Form 298 (Rev. 8/98) Prescribed by ANSI Std. Z39.18
|
||
|
||
ii
|
||
|
||
Contents
|
||
|
||
List of Figures
|
||
|
||
iv
|
||
|
||
Acknowledgments
|
||
|
||
v
|
||
|
||
1. Introduction
|
||
|
||
1
|
||
|
||
2. Problem Formulation
|
||
|
||
3
|
||
|
||
2.1 Coordinate Systems .........................................................................................................3
|
||
|
||
2.2 Beacon Position and Attitude Navigation Aided by Magnetometer ...............................4
|
||
|
||
2.3 Magnetometer Model ......................................................................................................7
|
||
|
||
2.4 Iterative Position and Attitude Estimation ......................................................................7
|
||
|
||
3. Position Estimation Based on 3-D AOA Vectors
|
||
|
||
9
|
||
|
||
3.1 2-D AOA Location Algorithm ........................................................................................9
|
||
|
||
3.2 Position Estimation Based on 3-D AOA Measurements...............................................11
|
||
|
||
4. Attitude Estimation Algorithm Based on 2-D DOA Estimation
|
||
|
||
13
|
||
|
||
4.1 A Fast 2-D DOA Estimation Algorithm........................................................................13
|
||
|
||
4.2 Attitude Estimation Algorithm Based on Fast 2-D DOA Estimation ...........................15
|
||
|
||
5. Simulations
|
||
|
||
18
|
||
|
||
5.1 Performance-Enhancement Techniques ........................................................................18
|
||
|
||
5.2 Simulations of Iterative Position and Attitude Estimation Algorithms.........................18
|
||
|
||
6. Conclusion
|
||
|
||
21
|
||
|
||
7. References
|
||
|
||
22
|
||
|
||
Appendix. Fast Two-Dimensional Direction of Arrival Estimation Algorithm
|
||
|
||
25
|
||
|
||
Distribution List
|
||
|
||
28
|
||
|
||
iii
|
||
|
||
List of Figures
|
||
Figure 1. Earth- and body-fixed coordinate systems and the Euler angle rotations. ......................3 Figure 2. Position and attitude estimation based on ground-based beacons...................................5 Figure 3. The ULAs equipped on the object receive the impinging signals...................................5 Figure 4. Iterative position and attitude estimation algorithm based on beacons and a
|
||
magnetometer. ............................................................................................................................9 Figure 5. Coordinates for the analytic geometry solution of the 2-D AOA position location......10 Figure 6. The 3-D coordinates are projected into the 2-D X-Y plane. .........................................12 Figure 7. The proposed array configuration equipped on the object. ...........................................14 Figure 8. Relative locations between the trajectory of the object and the three beacons. ............19 Figure 9. Simulation result for the estimation of x0. SNR = 20 dB. MSE = 2.63 × 104. ............19 Figure 10. Simulation result for the estimation of y0. SNR = 20 dB. MSE = 1.11 × 104. ..........19 Figure 11. Simulation result for the estimation of z0. SNR = 20 dB. MSE = 2.37 × 105. ..........20 Figure 12. Simulation result for the estimation of pitch angle. SNR = 20 dB. MSE =
|
||
1.31 × 10-6. ...............................................................................................................................20 Figure 13. Simulation result for the estimation of yaw angle. SNR = 20 dB. MSE =
|
||
3.37 × 10-4. ...............................................................................................................................20 Figure 14. Simulation result for the estimation of roll angle. SNR = 20 dB. MSE =
|
||
2.97 × 10-1. ...............................................................................................................................21 Figure A-1. One impinging signal is received by the system of three parallel ULAs
|
||
equipped on the object. ............................................................................................................25 Figure A-2. One impinging signal determines a cone, on whose surface infinite body-
|
||
fixed coordinates can be found to satisfy the estimated elevation and azimuth angles. ..........26 Figure A-3. Two impinging signals determine a pair of attitudes of the object. ..........................27
|
||
iv
|
||
|
||
Acknowledgments
|
||
The authors wish to express thanks to the following individuals for their contributions to this research. Dr. Gary Katulka (ARL) for serving as the program manager (contracting officer’s representative) of the University of Delaware contractual effort, and for technical concept formulation for aspects of the project; Mr. Richard McGee for providing assistance during the execution of the contract in addition to concept formulation; Mr. Tom Harkins for providing a thorough technical review of the final manuscript; and lastly, Mr. David Lyon is acknowledged for program support, motivation, and concept formulation during the project.
|
||
v
|
||
|
||
INTENTIONALLY LEFT BLANK. vi
|
||
|
||
1. Introduction
|
||
Position and attitude parameter estimation is often a prerequisite for controlling aerospace mechanical systems moving in space (1). Because projectile position often cannot be accurately estimated a priori due to various hardware and environmental uncertainties, guidance, navigation and control (GNC) systems are deployed onboard the object.
|
||
Rocket-propelled missile GNC systems are quite mature (2, 3), while effective GNC systems for accurate microunmanned vehicles are under development. For accurate, low-cost, and real-time applications, the design and implementation of the GNC systems are constrained by several technical challenges. Cost is by far the most challenging requirement, while computational efficiency is also required. In addition, robustness is needed in some cases for the desired GNC system to survive extremely harsh environments, such as gun-launched projectiles. Small, compact, and energy-efficient structures are also required for certain applications.
|
||
The primary function of position estimation is to locate the coordinates of an object with respect to a set of reference points (4). In general, position estimation algorithms can be classified into three groups: methods based on time of arrival (TOA), time difference of arrival (TDOA), and angle of arrival (AOA). TOA methods capture the traveling time of signals between the object and the reference points, from which the distances between them are determined. Subsequently, the position of the object is estimated by the geometric relationship (5, 6). In order to measure the traveling time of signals, TOA systems need the accurate synchronization between the object and the reference points. TDOA methods determine the position of the object by examining the difference in time at which the signal arrives at multiple reference points (4). Thus TDOA systems require the accurate synchronization between the reference points. In addition, since the electromagnetic waves propagate at the constant speed of light, both TOA and TDOA systems demand a very high sampling rate to obtain high resolution of the time arrival. Similar to a global positioning system (GPS), the requirements of accurate synchronization and high sampling rate make TOA and TDOA systems expensive. AOA positioning, on the other hand, allows low-cost implementation. It determines the position of an object by the use of the estimated angles between the object and a set of reference points (7–9). A variety of algorithms have been proposed to estimate the angle of arrival (10–17).
|
||
The primary function of the attitude estimation system is to determine the relative orientation between the coordinate frame fixed to the object and another reference coordinate frame (18). The attitude of the object can be described in three ways: direction cosine matrix (DCM), quaternion of rotation, and Euler angles (19). Wahba (20) first addressed the problem of determining the attitude of a rotating body by the vector-matching method. Subsequently, attitude estimation methods have relied mostly on the DCM (21–23). When the DCM description is used, the attitude parameters are linearly related to the observed vectors. Thus
|
||
1
|
||
|
||
linear estimation theory can be applied to the estimation of the DCM. DCM, however, uses nine parameters to describe the attitude, which increases computational complexity. The quaternion of rotation method applies Euler’s theorem, which states that any real rotation of one coordinate frame with respect to another may be described by a rotation through some angle about a single fixed axis (24). The quaternion method reduces the number of attitude parameters from nine to four. However, the quaternion parameters are nonlinearly related to the observed vectors. Some nonlinear approaches were proposed to determine the quaternion parameters, such as the Quest algorithm and quaternion Kalman filters (25–27). The third approach of attitude estimation uses Euler angles. The use of Euler angles is advantageous since the steering commands are explicit functions of the object’s Euler angles. In addition, the Euler angle method of attitude estimation requires determining only three parameters. Euler angles, however, are nonlinearly related to the observed vectors, and the attitude tracking using Euler angles is perhaps hampered by possible singularities (28, 29). Recently, Wilson (30) developed a novel nonlinear approach to estimate the Euler angles of the projectile. However, this proposed system needs three expensive angular rate sensors. Subsequently, Wilson (31) extended this work and designed an attitude estimation system only based on a set of magnetometers. This approach, however, requires the exact knowledge of the object’s dynamic model with properly modeled forces and moments that sometimes are not observed.
|
||
This report proposes a position and attitude navigation system, where both position and attitude can be iteratively estimated based on small and inexpensive sensor devices, such as uniform linear arrays (ULAs) and a magnetometer. It will be explained in section 2.4 that the position or attitude of the object cannot be estimated alone based on N beacons or a magnetometer—both are required. In addition, the algorithm for simultaneously estimating the position and attitude is not robust. Therefore, an iterative position and attitude estimation algorithm is developed in this report. In this iterative algorithm, the position is determined by AOA vector observations, and attitude is tracked using the Euler angle method.
|
||
The system is accomplished in several steps. A set of ULAs on the object receiving the impinging RF signals transmitted from ground-based beacons is used to estimate the directions of arrival of the signals. In addition, since the signal-to-noise ratio (SNR) of the magnetometer is usually high, an aiding magnetometer is equipped on the spin axis of the object to improve the performance of the position and attitude estimation. The ULA and magnetometer measurements are exploited to estimate the position of the object based on the prior knowledge of the initial attitude parameters. Based on the position estimation results, the attitude parameters are estimated and updated. The resulting attitude parameter estimates are used to approximate the position parameters in the next estimation cycle. The equations in the algorithms have been developed assuming an ideal vertical magnetic field vector, therefore additional work in the form of coordinate transforms would be required for a generalized solution, which is not specifically addressed in this report. The proposed system is low-cost, and the estimates are simple and thus useful when GPS and expensive inertial measurement units (IMUs) are not available.
|
||
2
|
||
|
||
The report is organized as follows. The problem formulation is discussed in section 2. The position estimation based on three-dimensional (3-D) AOA location algorithm is developed in section 3. The attitude estimation algorithm based on two-dimensional (2-D) direction of arrival (DOA) estimation is developed in section 4. Simulations of the proposed algorithms are presented in section 5. Section 6 provides conclusions.
|
||
2. Problem Formulation
|
||
2.1 Coordinate Systems The motion of an object is usually described by rigid body equations of motion derived from Newton’s laws (29). This section summarizes and notates three kinds of coordinate systems. The first is the Earth-fixed coordinate system, which is fixed to the Earth with a flat Earth assumption. Denote X, Y, and Z as the unit vectors pointing in the directions of the X, Y, and Z axes, respectively. Without loss of generality, the X, Y, and Z axes point to forward, right, and down, respectively. The second is the body-fixed coordinate system, with three unit vectors Xb, Yb, and Zb pointing to the Xb, Yb, and Zb axes, respectively. The Xb axis is along the object’s symmetric axis, referred to as the spin axis. The other two axes are perpendicular to the spin axis and each other. The Earth-fixed coordinate system and the body-fixed coordinate system are shown in figure 1. The third is the fixed-plane system, which is a body-fixed system that does not take the spin into account. The three axes are the Xp, Yp, and Zp axes, where Xp is overlapped with the Xb axis, and the Yp axis lies in the horizontal with respect to the Earth.
|
||
Figure 1. Earth- and body-fixed coordinate systems and the Euler angle rotations.
|
||
3
|
||
|
||
The Earth- and body-fixed coordinate systems are related by a Euler rotation sequence. The Earth-fixed system is first rotated about the Z axis through the yaw angle ψ, then about the new Y axis through the pitch angle θ, finally about the new X axis though the roll angle φ, which is shown in figure 1. From the Earth-fixed to body-fixed system, the transformation matrix is
|
||
|
||
|
||
|
||
c c
|
||
|
||
s c
|
||
|
||
s
|
||
|
||
Teb c s s s c s s s c c c s ,
|
||
|
||
(1)
|
||
|
||
|
||
|
||
c
|
||
|
||
s
|
||
|
||
c
|
||
|
||
|
||
|
||
s
|
||
|
||
s
|
||
|
||
s s c c s
|
||
|
||
c
|
||
|
||
c
|
||
|
||
|
||
|
||
where s• is sin(•) and c• is cos(•). If the roll angle is fixed as φ = 0, the transformation matrix from the Earth-fixed to fixed-plane system is
|
||
|
||
c c s c s
|
||
|
||
Tep
|
||
|
||
|
||
|
||
|
||
|
||
s c s
|
||
|
||
c s s
|
||
|
||
0 . c
|
||
|
||
(2)
|
||
|
||
The transformation matrix from the fixed-plane to body-fixed system is
|
||
|
||
1 0 0
|
||
|
||
Tep
|
||
|
||
|
||
|
||
|
||
|
||
0
|
||
|
||
c
|
||
|
||
s
|
||
|
||
|
||
|
||
.
|
||
|
||
(3)
|
||
|
||
0 s c
|
||
|
||
2.2 Beacon Position and Attitude Navigation Aided by Magnetometer
|
||
|
||
As shown in figure 2, suppose the position of an object is (x0,y0,z0) in 3-D free space and the
|
||
|
||
attitude of the object is (θ, ψ, φ). N beacons are located on the ground at known positions
|
||
|
||
(xi,yi,zi), i = 1,...,N, serving as reference points for the position and attitude navigation. The
|
||
|
||
beacons emit RF transmitted from at frequencies f1,
|
||
|
||
tfsh2i,ge..n.b,aeflNas,cworeintsshpaethrceetiirvreeaplnyret.esneTnnhtaeedpuanbtitytertvhneesctsuopnraitnsnvi ieinscgtroe3rfs-eDrorfefrdse1et,osspa2sa,.ct.he.,.esiTNthh,eAeaNOchsAigwvnoearclktsoinr.g
|
||
|
||
As shown in figure 2, in the Earth-fixed coordinate, the elevation angles of the AOA vectors are
|
||
|
||
a1, a2,..., aN, and the azimuth angles of the AOA vectors are b1, b2,..., bN. On the object, N sets of
|
||
|
||
ULAs are equipped to receive impinging signals transmitted from the beacons. In order to
|
||
|
||
distinguish the N signals, these ULAs are applied with different work frequencies of f1, f2,..., fN,
|
||
|
||
corresponding to different signals. The structures of the equipped ULAs are shown in figure 3.
|
||
|
||
As shown in figure 3, in the body-fixed coordinate, the elevation angles of the AOA vectors are
|
||
|
||
α1, α2,..., αN, and the azimuth angles are β1, β2,..., βN. The directivities of the AOA vectors in the
|
||
|
||
Earth-fixed coordinate are described by the elevation and azimuth angles (ai,bi), while the
|
||
|
||
directivities of the AOA vectors in the body-fixed coordinate are described by the elevation and
|
||
|
||
azimuth angles (αi,βi), where i = 1, 2,...,N. In addition, a magnetometer is equipped on the spin
|
||
|
||
axis of the object to measure the magnetic field vector in the body-fixed coordinate. The magnetic
|
||
|
||
field vector in the Earth-fixed coordinate is along the Z-axis in figure 1.
|
||
|
||
4
|
||
|
||
Figure 2. Position and attitude estimation based on ground-based beacons.
|
||
Figure 3. The ULAs equipped on the object receive the impinging signals.
|
||
5
|
||
|
||
The voltage measurements from the N sets of ULAs are exploited by a 2-D DOA estimation
|
||
|
||
algorithm, obtaining the estimation of the N pairs of elevation and azimuth angles in the body-fixed coordinate: (ˆ1, ˆ1), (ˆ2 , ˆ2 ),..., (ˆN , ˆN ) . The ith pair of the elevation and azimuth angles (ˆi , ˆi ) is referred to as the DOA of the ith AOA vector in the body-fixed coordinate.
|
||
Thus, the ULAs can only measure the AOA vectors in the body-fixed coordinate. In the
|
||
|
||
body-fixed coordinate,
|
||
|
||
si
|
||
|
||
|
||
|
||
(sinˆ i
|
||
|
||
cos
|
||
|
||
ˆi
|
||
|
||
sin ˆi
|
||
|
||
sin
|
||
|
||
ˆi
|
||
|
||
cosˆi )T ,i
|
||
|
||
1,2,..., N
|
||
|
||
.
|
||
|
||
(4)
|
||
|
||
On the other hand, in the Earth-fixed coordinate,
|
||
|
||
si
|
||
|
||
|
||
|
||
(sin
|
||
|
||
ai
|
||
|
||
cos
|
||
|
||
bi
|
||
|
||
sin
|
||
|
||
ai
|
||
|
||
sin
|
||
|
||
bi
|
||
|
||
cos
|
||
|
||
ai )T
|
||
|
||
,i
|
||
|
||
1, 2, ...,
|
||
|
||
N
|
||
|
||
.
|
||
|
||
(5)
|
||
|
||
A relationship between the AOA vectors in the body-fixed coordinate and in the Earth-fixed coordinate is described as follows. When the transformation relationship between the Earthfixed and body-fixed coordinates is applied in equation 1,
|
||
|
||
|
||
|
||
sin ˆi cos
|
||
sin ˆi sin cos ˆi
|
||
|
||
ˆi ˆi
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
c c
|
||
|
||
s s
|
||
|
||
c s c
|
||
|
||
c
|
||
|
||
s s
|
||
|
||
c s
|
||
|
||
s c s s s c c s s c c s
|
||
|
||
s c s c c
|
||
|
||
|
||
|
||
|
||
|
||
sin ai cos sin ai sin
|
||
cos ai
|
||
|
||
bi bi
|
||
|
||
|
||
|
||
,
|
||
|
||
(6)
|
||
|
||
for i =1, 2,...,N. Thus, we have
|
||
|
||
u = Hv ,
|
||
|
||
(7)
|
||
|
||
where
|
||
|
||
u [u1T ,uT2 ,...,uTN ]T andui (sinˆi cos ˆi , sinˆi sin ˆi , cosˆi )T ,
|
||
|
||
(8)
|
||
|
||
v [v1T , vT2 ,..., vTN ]T and vi (sin ai cos bi , sin ai sin bi , cos ai )T , i 1, 2,..., N ,
|
||
|
||
(9)
|
||
|
||
and
|
||
|
||
H
|
||
|
||
|
||
|
||
|
||
|
||
Teb1 0
|
||
|
||
0
|
||
Teb 2
|
||
|
||
|
||
|
||
0
|
||
|
||
0
|
||
|
||
|
||
|
||
|
||
|
||
.
|
||
|
||
(10)
|
||
|
||
|
||
|
||
0
|
||
|
||
0
|
||
|
||
|
||
|
||
TebN
|
||
|
||
A transformation of equation 6 is
|
||
|
||
sin ai cos bi c c
|
||
|
||
sin ai sin bi c s s s c
|
||
|
||
|
||
|
||
cos ai
|
||
|
||
|
||
|
||
|
||
|
||
c
|
||
|
||
s
|
||
|
||
c
|
||
|
||
|
||
|
||
s
|
||
|
||
s
|
||
|
||
s c s s s c c s s c c s
|
||
|
||
s c s c c
|
||
|
||
|
||
|
||
1
|
||
|
||
|
||
|
||
sinˆi cos sin ˆi sin
|
||
cosˆi
|
||
|
||
ˆi ˆi
|
||
|
||
|
||
|
||
,
|
||
|
||
(11)
|
||
|
||
where i = 1, 2,…,N.
|
||
|
||
6
|
||
|
||
2.3 Magnetometer Model
|
||
The magnetometer is a device that produces a voltage output proportional to the applied magnetic field in the direction of the magnetometer’s sensitive axis. The magnetometer is ideal for broad applications because it is very rugged and low cost. Let the sensitive axis be a unit vector i, and the local magnetic field vector is B. Then the measurement of an ideal magnetometer is the inner product of i and B
|
||
|
||
Mideal = i · B .
|
||
|
||
(12)
|
||
|
||
When the scale and shift influences of the analog circuitry are considered, the actual measurement of the magnetometer is
|
||
|
||
M = s(i · B) + b ,
|
||
|
||
(13)
|
||
|
||
where s is the scale factor and b is the bias offset. If the included angle between i and B is θ, equation 13 is rewritten as
|
||
|
||
M = sBcosθ + b ,
|
||
|
||
(14)
|
||
|
||
where B = |B|. Thus,
|
||
|
||
|
||
|
||
|
||
|
||
cos1
|
||
|
||
|
||
|
||
M b sB
|
||
|
||
|
||
|
||
.
|
||
|
||
(15)
|
||
|
||
In general, the SNR of the magnetometer is very high. However, some amount of noise is always present, which is modeled as white Gaussian noise (WGN). Therefore, the measurement of the magnetometer is adjusted as
|
||
|
||
M = sBcosθ + b + nM ,
|
||
|
||
(16)
|
||
|
||
where nM is WGN with zero mean and a variance of σ2M.
|
||
|
||
2.4 Iterative Position and Attitude Estimation
|
||
|
||
As described in section 2.2, the proposed navigation system measures one magnetic field vector and N AOA vectors. Based on the measurement vectors, it is proper to ask if these are sufficient to estimate either the position or the attitude. Consider the position estimation first. It will be proven in section 3.2 that three linearly independent vectors in the Earth-fixed coordinate with reference points are necessary and sufficient to determine the position of an object in 3-D free space. Since no reference point is associated to the magnetic field vector, it does not contribute to the estimation of position. Thus, the N AOA vectors must be used in the resection algorithm to estimate the position of the object (7). As mentioned in section 2.2, the ULAs equipped on the object can only measure the AOA vectors in the body-fixed coordinate. In order to obtain the AOA vectors in the Earth-fixed coordinate, the elevation and azimuth angles (ai,bi) need to
|
||
|
||
7
|
||
|
||
be estimated from equation 11. In equation 11, the attitude parameters (θ, ψ, φ) are needed to calculated (ai,bi). Thus, the position parameters cannot be estimated alone without attitude estimation.
|
||
|
||
Next, consider the attitude estimation. It has been proven that two pairs of linearly independent vectors are necessary and sufficient to determine the attitude of an object (21). That means we have to know the two linearly independent vectors in both the Earth-fixed coordinate and the body-fixed coordinate. As described in section 2.2, the magnetic field vector is known in both coordinates, thus it provides one pair of vectors for the attitude estimation. In order to estimate the attitude parameters, we need to find another pair of linearly independent vectors from the N AOA vectors. Although the ULAs measure the AOA vectors in the body-fixed coordinate, we cannot obtain the AOA vectors in the Earth-fixed coordinate without the position of the object. Thus, we cannot find another pair of linearly independent vectors without position estimation. In an attitude estimation system, position information is usually obtained by GPS (18). Based on the previous analysis, the position or attitude cannot be estimated alone.
|
||
|
||
Given that the position and attitude must be estimated jointly, the next question to address is if the position and attitude can be estimated simultaneously. The magnetometer measurement is shown in equation 16. The ULA measurements are described in equation 6, which can be rewritten as
|
||
|
||
|
||
|
||
sinˆi cos ˆi sinˆi sin ˆi
|
||
cosˆi
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
c c
|
||
|
||
s s
|
||
|
||
c s c
|
||
|
||
c
|
||
|
||
s s
|
||
|
||
c s
|
||
|
||
s c s s s c c s s c c s
|
||
|
||
s c s
|
||
|
||
|
||
|
||
|
||
|
||
( (
|
||
|
||
xi yi
|
||
|
||
|
||
|
||
x0 ) y0 )
|
||
|
||
normi normi
|
||
|
||
|
||
|
||
,
|
||
|
||
c c (zi z0 ) normi
|
||
|
||
(17)
|
||
|
||
where normi = { (xi − x0)2 + (yi − y0)2 + (zi − z0)2 }1/2. Thus, equations 16 and 17 consist of a system of equations, where m = (x0,y0,z0, θ, ψ, φ)T are the unknown position and attitude parameters to be estimated. Equations 16 and 17 are nonlinear equations, where the unknown parameters are coupled together. In the simultaneous position and attitude estimation process, the parameter vector m must be estimated using recursive algorithms, such as a nonlinear least squares estimation algorithm. Our extensive simulations show that these simultaneous position and attitude estimation algorithms are not robust. If the SNR of the ULAs is smaller than 25 dB, the algorithm exploiting four beacons does not converge with very high probability.
|
||
|
||
While the simultaneous position and attitude estimation is not robust, iterative position and
|
||
|
||
attitude estimation algorithms are proposed next based on beacons and a magnetometer. Suppose the initial attitude of the object before launch is known as (θ∗,ψ∗,φ∗). At the beginning
|
||
|
||
of the estimation process, the initial attitude parameters of (φ, ψ, θ) are used to solve the
|
||
|
||
e(leˆiv,abˆtii)o,nthaendpoasziitmiounthofa(nxg0l,eys0,z(0)ˆic,abˆni )
|
||
|
||
according to equation 9. Based be subsequently estimated based
|
||
|
||
on on
|
||
|
||
the the
|
||
|
||
estimation 3-D AOA
|
||
|
||
results of location
|
||
|
||
algorithm proposed in section 3.2. Based on equations 16 and 17 and the position estimation
|
||
|
||
8
|
||
|
||
results, the attitude parameters of (θ, ψ, φ) can be updated by the nonlinear estimation approach proposed in section 4.2. Then the current attitude estimation results will be exploited to update (ˆi ,bˆi ) in the next iteration. The scheme of the iterative position and attitude estimation algorithms is illustrated in figure 4. The details of the position and attitude estimation algorithm are presented in sections 3 and 4.
|
||
Figure 4. Iterative position and attitude estimation algorithm based on beacons and a magnetometer.
|
||
In section 3.2, it is shown that three AOA vectors are necessary and sufficient to determine the position of the object. In the appendix, it is shown geometrically that two AOA vectors are necessary and sufficient to determine the attitude of the object. Therefore, in order to estimate both position and attitude, the proposed algorithms use three ground-based beacons. Since the magnetometer has very high SNR, an aiding magnetometer measures an additional magnetic field vector to improve the performance of the position and attitude estimation.
|
||
3. Position Estimation Based on 3-D AOA Vectors
|
||
3.1 2-D AOA Location Algorithm In this section, the 2-D AOA location algorithm will be first summarized and exploited as the foundation of the proposed 3-D AOA location algorithm. The 2-D AOA position algorithm, referred to as the resection algorithm, was proposed by Rappaport et al. (4) and McGillem and Rappaport (7). The coordinates for the analytic geometry solution of the AOA position location are shown in figure 5, where the position of points A, B, and C are (x1,y1), (x2,y2), and (x3,y3), respectively. The angle between AP and BP is γ1. The angle between BP and CP is γ2. The angle between AB and the horizontal direction is τ1. The angle between BC and the horizontal direction is τ2. Let the position of P be (x0,y0), which is calculated based on the following known parameters:
|
||
9
|
||
|
||
Figure 5. Coordinates for the analytic geometry solution of the 2-D AOA position location.
|
||
|
||
y0
|
||
|
||
|
||
|
||
2mxN
|
||
|
||
2 yN 2mn 1 m2
|
||
|
||
y2
|
||
|
||
(18)
|
||
|
||
and
|
||
|
||
x0 my0 n ,
|
||
|
||
(19)
|
||
|
||
where
|
||
|
||
m
|
||
|
||
yM xN
|
||
|
||
yN xM
|
||
|
||
,
|
||
|
||
(20)
|
||
|
||
n R22
|
||
|
||
R12
|
||
|
||
xM2
|
||
|
||
|
||
|
||
x
|
||
|
||
2 N
|
||
|
||
yM2
|
||
|
||
|
||
|
||
y
|
||
|
||
2 N
|
||
|
||
2(xN xM )
|
||
|
||
,
|
||
|
||
(21)
|
||
|
||
xN x1 R1 sin(1 1 ) ,
|
||
|
||
(22)
|
||
|
||
y N y1 R1 cos( 1 1 ) ,
|
||
|
||
(23)
|
||
|
||
xM x2 R2 sin( 2 2 ) ,
|
||
|
||
(24)
|
||
|
||
yM x2 R2 cos( 2 2 ) ,
|
||
|
||
(25)
|
||
|
||
R1
|
||
|
||
|
||
|
||
AB 2 sin 1
|
||
|
||
,
|
||
|
||
(26)
|
||
|
||
10
|
||
|
||
and
|
||
|
||
R2
|
||
|
||
|
||
|
||
BC 2 sin 2
|
||
|
||
.
|
||
|
||
(27)
|
||
|
||
For the case of x0 my0 n , equations 18 and 19 maybe equivalently expressed as
|
||
|
||
x0
|
||
|
||
|
||
|
||
2m' yN
|
||
|
||
2xN 2m'n' 1 m'2
|
||
|
||
x2
|
||
|
||
(28)
|
||
|
||
and
|
||
|
||
y0 m' x0 n' ,
|
||
|
||
(29)
|
||
|
||
where
|
||
|
||
m'
|
||
|
||
|
||
|
||
1 m
|
||
|
||
and
|
||
|
||
n'
|
||
|
||
|
||
|
||
n m
|
||
|
||
.
|
||
|
||
3.2 Position Estimation Based on 3-D AOA Measurements
|
||
|
||
In this section, a 3-D AOA location algorithm is generalized from the 2-D AOA location
|
||
|
||
algorithm described in section 3.1. It is noted that the resection algorithm requires at least three
|
||
|
||
AOA vectors to solve for the location in two dimensions. Suppose three beacons at known
|
||
|
||
positions are available. It will be shown in the following section that the three beacons are
|
||
|
||
sufficient to solve for the location in three dimensions, if the attitude of the object is assumed to be known as (ˆ,ˆ ,ˆ) . In order to estimate the position of the object in 3-D free space, the 3-D
|
||
|
||
coordinates are first projected into the 2-D X-Y plane, as illustrated in figure 6, where the three
|
||
|
||
beacons are located at points A(x1,y1), B(x2,y2), and C(x3,y3), respectively. The position of the
|
||
|
||
object in the X-Y plane is P(x0,y0). The included angle between AP and BP is |b2 − b1|, and the
|
||
|
||
isn1c,lus2d,eadnadngsl3eibnetthweeEenartBhP-fixaendd
|
||
|
||
CP is |b3 − coordinate,
|
||
|
||
b2|, where b1, respectively.
|
||
|
||
b2, and b3 However,
|
||
|
||
are the azimuth angles of when the position of the
|
||
|
||
object is unknown, b1, b2, and b3 cannot be determined. Therefore, b1, b2, and b3 need to be
|
||
|
||
estimated from the attitude estimation results. According to equation 11, we have
|
||
|
||
|
||
|
||
sin aˆi cos bˆi sin aˆi sin bˆi
|
||
cos aˆi
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
sˆi1 sˆi 2 sˆi3
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
cˆ cˆ
|
||
|
||
cˆ sˆ sˆ sˆ cˆ
|
||
|
||
cˆ
|
||
|
||
|
||
sˆ sˆ
|
||
|
||
cˆ sˆ
|
||
|
||
sˆ cˆ sˆ sˆ sˆ cˆ cˆ
|
||
sˆ sˆcˆ cˆ sˆ
|
||
|
||
sˆ cˆ sˆ
|
||
cˆcˆ
|
||
|
||
1
|
||
|
||
|
||
|
||
sinˆi cos
|
||
sinˆi sin cosˆi
|
||
|
||
ˆi ˆi
|
||
|
||
|
||
|
||
,
|
||
|
||
(30)
|
||
|
||
11
|
||
|
||
Figure 6. The 3-D coordinates are projected into the 2-D X-Y plane.
|
||
|
||
where i = 1, 2, 3. are the estimation
|
||
|
||
In of
|
||
|
||
equation 30, ˆ , ˆ , and ˆ are the attitude elevation and azimuth angles of si in the
|
||
|
||
estimation body-fixed
|
||
|
||
results, and coordinate,
|
||
|
||
ˆi and ˆi
|
||
obtained
|
||
|
||
from the fast 2-D DOA estimation algorithm described in section 4.1. According to equation 30,
|
||
|
||
ai and bi are estimated as follows:
|
||
|
||
aˆi cos1(sˆi3 )
|
||
|
||
(31)
|
||
|
||
and
|
||
|
||
tan1(sˆi2 sˆi1)
|
||
|
||
:
|
||
|
||
sˆi 2 sin aˆi
|
||
|
||
|
||
|
||
0
|
||
|
||
|
||
|
||
sˆi1 sin aˆi
|
||
|
||
0
|
||
|
||
bˆi
|
||
|
||
tan1(sˆi2 sˆi1) 2
|
||
|
||
:
|
||
|
||
sˆi2 sin aˆi
|
||
|
||
|
||
|
||
0
|
||
|
||
|
||
|
||
sˆi1 sin aˆi
|
||
|
||
0
|
||
|
||
.
|
||
|
||
tan1(sˆi2 sˆi1) :
|
||
|
||
otherwise
|
||
|
||
(32)
|
||
|
||
These parameters are applied for the resection algorithm described in section 3.1 to estimate xˆ0
|
||
|
||
and
|
||
|
||
yˆ0 , where 1
|
||
|
||
bˆ2 bˆ1
|
||
|
||
,
|
||
|
||
2 bˆ3 bˆ2
|
||
|
||
,
|
||
|
||
1 tan 1
|
||
|
||
y2 x2
|
||
|
||
|
||
|
||
y1 x1
|
||
|
||
and
|
||
|
||
2
|
||
|
||
|
||
|
||
tan 1
|
||
|
||
y3 y2 x3 x2
|
||
|
||
|
||
|
||
.
|
||
|
||
In addition, the altitude of the object z0 may be estimated from the geometric relationship
|
||
|
||
12
|
||
|
||
tan 2
|
||
|
||
aˆi
|
||
|
||
|
||
|
||
( xˆ 0
|
||
|
||
xi )2 ( yˆ0 (zˆ0 zi )2
|
||
|
||
yi )2
|
||
|
||
.
|
||
|
||
(33)
|
||
|
||
Thus
|
||
|
||
zˆ0i zi
|
||
|
||
(xˆ0 xi )2 ( yˆ0 yi )2 ,
|
||
tan 2 aˆi
|
||
|
||
(34)
|
||
|
||
where i = 1, 2, 3. When the mean estimation is applied, zˆ0 can be estimated as
|
||
|
||
zˆo
|
||
|
||
|
||
|
||
1 3
|
||
|
||
3 i 1
|
||
|
||
zˆoi
|
||
|
||
.
|
||
|
||
(35)
|
||
|
||
Therefore, three beacons are necessary and sufficient to estimate the position of the object in 3-D free space.
|
||
|
||
4. Attitude Estimation Algorithm Based on 2-D DOA Estimation
|
||
|
||
4.1 A Fast 2-D DOA Estimation Algorithm
|
||
Several 2-D DOA estimation algorithms have been presented in the literature. Maximum likelihood methods with high computational complexity were first proposed (10, 11). MUSIC and ESPRIT algorithms exploited either eigen value decomposition or singular value decomposition to obtain high resolution in DOA estimation (12–15). Wu et al. (16) proposed a 2-D DOA estimation algorithm based on propagator method. Recently, Tayem and Kwon (17) proposed a computationally efficient azimuth and elevation angle estimation algorithm with no failure and no eigen decomposition. The computational efficiency and accuracy of this algorithm are suitable for the real-time estimation of a rapidly moving object. This fast 2-D DOA estimation algorithm is summarized in the appendix. It will be exploited to formulate the position and attitude estimation algorithms.
|
||
Figure 7 shows the proposed array configuration consisting of three ULAs with interspacing d equal to a half wavelength of incident signals. The three ULAs consist of N, N +1, and N elements, respectively. The first and third ULAs are referred to as subarrays Z and W, respectively. The first and last N elements in the second ULA are referred to as subarrays X and Y, respectively. Thus, subarrays X, Y, Z, and W consist of N elements. Suppose there are K narrow-band sources, s(t) = [s1(t),s2(t),...,sK (t)]T , with the same wavelength λ impinging on the arrays. The received signals at the subarrays X, Y, Z, and W are
|
||
|
||
13
|
||
|
||
Figure 7. The proposed array configuration equipped on the object.
|
||
|
||
x(t)= As(t) + nx(t) ,
|
||
|
||
y(t)= AΦys(t) + ny(t) ,
|
||
|
||
z(t)= AΦzs(t) + nz(t) ,
|
||
|
||
w(t)= AΦws(t) + nw(t) ,
|
||
|
||
t =1, 2, . . . , L ,
|
||
|
||
(36)
|
||
|
||
where
|
||
|
||
A = [a(1, 1) , a(2, 2), …, a(K, K) ] ,
|
||
|
||
(37)
|
||
|
||
a( k
|
||
|
||
,
|
||
|
||
k
|
||
|
||
)
|
||
|
||
|
||
|
||
1,
|
||
|
||
exp
|
||
|
||
|
||
|
||
|
||
|
||
j
|
||
|
||
2
|
||
|
||
d
|
||
|
||
sin k
|
||
|
||
sin
|
||
|
||
k
|
||
|
||
|
||
|
||
,...,
|
||
|
||
exp
|
||
|
||
|
||
|
||
|
||
|
||
j
|
||
|
||
2(
|
||
|
||
N
|
||
|
||
|
||
|
||
1)
|
||
|
||
d sin
|
||
|
||
k
|
||
|
||
sin
|
||
|
||
k
|
||
|
||
T
|
||
|
||
,
|
||
|
||
(38)
|
||
|
||
y
|
||
|
||
diag[ p1, p2 ,..., pK ] , where
|
||
|
||
pK
|
||
|
||
exp
|
||
|
||
j 2d sink sin k
|
||
|
||
|
||
|
||
,
|
||
|
||
(39)
|
||
|
||
z
|
||
|
||
diag[q1, q2,..., qK ] ,
|
||
|
||
where
|
||
|
||
qK
|
||
|
||
exp
|
||
|
||
j
|
||
|
||
2d cos k
|
||
|
||
,
|
||
|
||
(40)
|
||
|
||
w
|
||
|
||
diag[r1, r2,..., rK ] , where
|
||
|
||
rK
|
||
|
||
exp
|
||
|
||
j 2d sink
|
||
|
||
cos k
|
||
|
||
|
||
|
||
,
|
||
|
||
(41)
|
||
|
||
where k = 1, 2,...,K. Let g(t) = [xT (t), yT (t), zT (t), wT (t)]T denote a 4N × 1 snapshot data vector with t = 1, 2,...,L. Let F = [g(1), g(2),..., g(L)] denote a 4N × L data matrix consisting of L snapshots. The partition of F is F = [F1T, F2T ]T , where F1 and F2 are submatrices with dimensions of K × L and (4N − K) × L, respectively. Let Pˆ = (F1F1H )−1F1F2H denote the K × (4N − K) propagator estimate matrix based on the data matrix F. The partitions of Pˆ are
|
||
|
||
14
|
||
|
||
Pˆ H ˆH = [Pˆ1T, Pˆ2T, Pˆ3T, Pˆ4T, Pˆ5T, Pˆ6T, Pˆ7T ]T, where the dimension of Pˆ1T, Pˆ3T, Pˆ5T and Pˆ7T is (N − K) × K, and the dimension of Pˆ2T, Pˆ4T, and Pˆ6T is K × K. Thus, the diagonal elements of Φy, Φz, and Φw, respectively, can be estimated by finding the K eigen values of Pˆ3 Pˆ1#, Pˆ5 Pˆ1# and Pˆ7 Pˆ1# , where #
|
||
denotes the pseudoinverse. Finally, the azimuth and elevation angles for each source can be estimated as
|
||
|
||
ˆk
|
||
|
||
|
||
|
||
tan
|
||
|
||
1
|
||
|
||
|
||
|
||
arg( arg(
|
||
|
||
y w
|
||
|
||
) )
|
||
|
||
kk kk
|
||
|
||
|
||
|
||
,
|
||
|
||
|
||
|
||
k = 1,2, …, K .
|
||
|
||
(42)
|
||
|
||
ˆ k
|
||
|
||
|
||
|
||
tan
|
||
|
||
1
|
||
|
||
|
||
|
||
arg( w )kk arg( z )kk cos
|
||
|
||
ˆk
|
||
|
||
|
||
|
||
,
|
||
|
||
|
||
|
||
k = 1,2, …, K .
|
||
|
||
(43)
|
||
|
||
4.2 Attitude Estimation Algorithm Based on Fast 2-D DOA Estimation
|
||
|
||
In this section, an attitude estimation algorithm is proposed based on the fast 2-D DOA estimation described in section 4.1. Suppose one magnetometer is equipped on the spin axis of the object. According to equation 15, and assuming b = 0 and sB = 1, the pitch angle (assuming a vertical magnetic field vector) is estimated as
|
||
|
||
ˆ sin1(M ) ,
|
||
|
||
(44)
|
||
|
||
where M is the measurement of the magnetometer.
|
||
|
||
On the other hand, it has been proven that two pairs of linearly independent vectors are necessary and sufficient to determine the attitude of the object (21). That means that given the position of the object, two ground-based beacons are necessary and sufficient to determine the attitude. The appendix gives a geometric explanation for this. Our proposed navigation system includes three beacons that are redundant to estimate the attitude. Corresponding to the three beacons, three sets of ULAs are equipped on the object to receive impinging signals. As described in section 3.2, the current position parameters have already been estimated as (xˆ0 , yˆ0 , zˆ0 ). Let the positions
|
||
of the three beacons be (x1,y1,z1), (x2,y2,z2), and (x3,y3,z3), respectively. Based on equation 17, we have
|
||
|
||
|
||
|
||
sin ˆi sin ˆi
|
||
|
||
cos ˆi sin ˆi
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
c
|
||
|
||
sˆ
|
||
|
||
c s
|
||
|
||
cˆ
|
||
|
||
s
|
||
|
||
c
|
||
|
||
cosˆi c sˆc s s
|
||
|
||
s cˆ s sˆ s c c s sˆc c s
|
||
|
||
sˆ cˆ s cˆ c
|
||
|
||
|
||
|
||
( xi ( yi ( zi
|
||
|
||
xˆ0 ) yˆ0 ) zˆ0 )
|
||
|
||
n orm i n orm i n orm i
|
||
|
||
|
||
|
||
,
|
||
|
||
(45)
|
||
|
||
where n ormi = { (xi – xˆ0 )2 ( yi – yˆ0 )2 (zi – zˆ0 )2 }1/2 and i = 1, 2, 3. Thus, we have
|
||
|
||
u = Hv ,
|
||
|
||
(46)
|
||
|
||
15
|
||
|
||
where
|
||
|
||
u [u1T ,uT2 ,...,uTN ]T andui (sinˆi cos ˆi , sinˆi sin ˆi , cosˆi )T ,
|
||
|
||
(47)
|
||
|
||
v [v1T , vT2 ,..., vTN ]T and vi (sin ai cos bi , sin ai sin bi , cos ai )T , i 1, 2,..., N ,
|
||
|
||
(48)
|
||
|
||
and
|
||
|
||
Teb 0 0
|
||
|
||
H
|
||
|
||
|
||
|
||
|
||
|
||
0
|
||
|
||
Teb
|
||
|
||
0
|
||
|
||
|
||
|
||
,
|
||
|
||
(49)
|
||
|
||
0 0 Teb
|
||
|
||
Let
|
||
|
||
sˆi1
|
||
|
||
|
||
|
||
xi xˆ0 n ormi
|
||
|
||
,
|
||
|
||
sˆi 2
|
||
|
||
|
||
|
||
yi yˆ0 n ormi
|
||
|
||
, and
|
||
|
||
sˆi3
|
||
|
||
|
||
|
||
zi zˆ0 n ormi
|
||
|
||
.
|
||
|
||
From the first equation of equation 45, we
|
||
|
||
have
|
||
|
||
cos( i
|
||
|
||
i)
|
||
|
||
|
||
|
||
sin ˆi
|
||
|
||
cos ˆi cˆ
|
||
|
||
|
||
|
||
sˆ sˆi3
|
||
|
||
,
|
||
|
||
(50)
|
||
|
||
where γ can be estimated as
|
||
|
||
tan1(sˆi2 sˆi1)
|
||
|
||
:
|
||
|
||
sˆi2 0 sˆi2 0
|
||
|
||
sˆi21 sˆi22
|
||
|
||
sˆi21 sˆi22
|
||
|
||
ˆi
|
||
|
||
tan1(sˆi2 sˆi1) 2 :
|
||
|
||
sˆi2 0 sˆi2 0 .
|
||
|
||
sˆi21 sˆi22
|
||
|
||
sˆi21 sˆi22
|
||
|
||
(51)
|
||
|
||
tan1(sˆi2 sˆi1) :
|
||
|
||
otherwise
|
||
|
||
Since the range of ˆi – ˆi is unknown, ˆi cannot be uniquely estimated from equation 50. The
|
||
|
||
estimation ˆi from the last iteration then provides an estimate of the range of ˆi
|
||
|
||
– ˆi .
|
||
|
||
Let
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
be
|
||
|
||
the estimation result of ψi in the kth iteration. Then in the k + 1th iteration,
|
||
|
||
ˆi 2
|
||
|
||
:
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
ˆi
|
||
|
||
[2,]
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
1
|
||
|
||
|
||
|
||
ˆi ˆi
|
||
|
||
:
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
|
||
|
||
ˆi
|
||
|
||
[,0]
|
||
|
||
:
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
|
||
|
||
ˆi
|
||
|
||
[0,]
|
||
|
||
,
|
||
|
||
(52)
|
||
|
||
ˆi 2
|
||
|
||
:
|
||
|
||
ˆ
|
||
|
||
k i
|
||
|
||
|
||
|
||
ˆi
|
||
|
||
[,2]
|
||
|
||
where
|
||
|
||
16
|
||
|
||
|
||
|
||
|
||
|
||
cos 1 sin ˆ i
|
||
|
||
cos ˆi cˆ
|
||
|
||
sˆ sˆi3
|
||
|
||
|
||
|
||
.
|
||
|
||
(53)
|
||
|
||
Averaging over the three sets of estimations, we have
|
||
|
||
ˆ
|
||
|
||
|
||
|
||
1 3
|
||
|
||
3
|
||
ˆ i
|
||
i 1
|
||
|
||
.
|
||
|
||
(54)
|
||
|
||
From the second and third equations of equation 45, we have
|
||
|
||
sinˆi cos ˆi m1 sin m2 cos cosˆi m3 sin m4 cos
|
||
|
||
,
|
||
|
||
(55)
|
||
|
||
where
|
||
|
||
m1 sin aˆi cos bˆicˆ sˆ sin aˆi sin bˆi sˆ sˆ cos aˆicˆ
|
||
|
||
m2 sin aˆi sin bˆicˆ sin aˆi cos bˆi sˆ
|
||
|
||
.
|
||
|
||
(56)
|
||
|
||
m3 sin aˆi cos bˆi sˆ sin aˆi sin bˆicˆ
|
||
|
||
m4 sin aˆi cos bˆicˆ sˆ sin aˆi sin bˆi sˆ sˆ cos aˆicˆ
|
||
|
||
Thus, for each i, sinˆi and cisˆi can be solved as
|
||
|
||
sin ˆi
|
||
|
||
|
||
|
||
|
||
|
||
m4
|
||
|
||
sin ˆ i sin ˆi m2 m1m4 m2 m3
|
||
|
||
cos ˆ i
|
||
|
||
.
|
||
|
||
(57)
|
||
|
||
cos ˆi
|
||
|
||
|
||
|
||
|
||
|
||
m3
|
||
|
||
sin ˆ i sin ˆi m1 m2 m3 m1m4
|
||
|
||
cos ˆ i
|
||
|
||
.
|
||
|
||
(58)
|
||
|
||
Therefore,
|
||
|
||
tan1(sinˆi cosˆi ) : sinˆi 0 cosˆi 0
|
||
|
||
ˆi tan1(sinˆi cosˆi ) 2 : sinˆi 0 cosˆi 0 .
|
||
|
||
(59)
|
||
|
||
tan 1(sˆi2 sˆi1)
|
||
|
||
:
|
||
|
||
otherwise
|
||
|
||
Applying an averaging operation over the three sets of estimations, we have
|
||
|
||
ˆ
|
||
|
||
|
||
|
||
1 3
|
||
|
||
3 i 1
|
||
|
||
ˆi
|
||
|
||
.
|
||
|
||
(60)
|
||
|
||
17
|
||
|
||
5. Simulations
|
||
|
||
5.1 Performance-Enhancement Techniques
|
||
|
||
In section 4.2, equations 54 and 60 are intended to give good estimations of the yaw and roll angles, respectively. However, when the denominators in equations 50 and 57–59 are small, the errors of the estimations are magnified. In addition, in the iterative position and attitude estimation algorithms, the error may be transferred between each other and accumulated. In order to further improve the estimation performance, a recursive median filter is applied in the estimation process to remove large deviations from the estimation results (32). The output of the recursive median filter with window width equal to 2N + 1 is
|
||
|
||
xˆM (t) median xˆM (t Nt),..., xˆM (t t), xˆ(t), xˆ(t t),..., xˆM (t Nt) ,
|
||
|
||
(61)
|
||
|
||
where xˆ is the arbitrary estimated parameter and Δt is the sampling interval. In the recursive
|
||
median filter, the prior outputs are used in the current filter step. As shown in equation 61, xˆM (t Nt),..., xˆM (t t) are prior outputs. Since the SNR of the magnetometer is usually very
|
||
high, the recursive median filter is not applied to the estimation of the pitch angle.
|
||
|
||
5.2 Simulations of Iterative Position and Attitude Estimation Algorithms
|
||
The proposed iterative position and attitude estimation algorithms are simulated using a gun-launched object. In these simulations, the object is launched with an initial pitch angle θ0 = 45°, initial yaw angle ψ0 = 0°, initial roll angle φ0 = 0°, and initial position at (0, 0, 0). The trajectory of the object is a standard parabola. The range of the trajectory is 3 km and the traveling time is 2 s. Three ground-based beacons are located at (–1500, 3000, 0), (4000, 1000, 0), and (6000, –4000, 0), respectively. The work frequencies of the three beacons are f1 = 10 GHz, f2 = 5 GHz, and f3 = 7.5 GHz, respectively. All the subarrays of the ULAs equipped on the object consist of N = 4 elements. Assume the noise on the ULAs is WGN, with SNR = 20 dB. The noise on the magnetometer is also WGN, with the variance σ2M = 10−6.
|
||
The relative locations between the trajectory of the object and the three beacons are illustrated in figure 8. Recursive median filters with window width equal to 7 are applied to the estimations of ψ, φ, x0, y0, and z0. The simulation results are shown in figures 9–14, corresponding to the estimations of coordinates on the X-axis, Y-axis, Z-axis, and pitch angle, yaw angle and roll angle, respectively.
|
||
|
||
18
|
||
|
||
Figure 8. Relative locations between the trajectory of the object and the three beacons.
|
||
Figure 9. Simulation result for the estimation of x0. SNR = 20 dB. MSE = 2.63 × 104.
|
||
Figure 10. Simulation result for the estimation of y0. SNR = 20 dB. MSE = 1.11 × 104.
|
||
19
|
||
|
||
Figure 11. Simulation result for the estimation of z0. SNR = 20 dB. MSE = 2.37 × 105.
|
||
Figure 12. Simulation result for the estimation of pitch angle. SNR = 20 dB. MSE = 1.31 × 10-6.
|
||
Figure 13. Simulation result for the estimation of yaw angle. SNR = 20 dB. MSE = 3.37 × 10-4.
|
||
20
|
||
|
||
Figure 14. Simulation result for the estimation of roll angle. SNR = 20 dB. MSE = 2.97 × 10-1.
|
||
|
||
In figures 9–14, the MSE 2.37 × 105, the MSE of ˆ
|
||
|
||
of is
|
||
|
||
xˆ0 is 1.31
|
||
|
||
2.63 × 104, × 10−6, the
|
||
|
||
the MSE of MSE of ˆ
|
||
|
||
isyˆ03.i3s71×.1110×−41,0a4n,dthteheMMSSEEofofzˆ0ˆ
|
||
|
||
is is
|
||
|
||
2.97 × 10−1. The recursive median filters successfully remove some large deviations from the
|
||
|
||
estimation results. However, it is noted that there are still some singular points in figures 9–11,
|
||
|
||
13, and 14, which singularity. First, to guess the range
|
||
|
||
result in the large MSEs of xˆ0 , yˆ0 , and zˆ0 . There are two reasons for the the denominators in equations 57–59 are close to zero. Second, the use of of ˆik1 ˆi in equation 52 can sometimes be inaccurate. In addition, the
|
||
|
||
ˆik
|
||
|
||
period of these deviations is longer than the window period of the used recursive median filters.
|
||
|
||
Therefore, the recursive median filters cannot remove these deviations. In our future research,
|
||
|
||
some approaches will be proposed to eliminate the singular points of the iterative position and
|
||
|
||
attitude estimation algorithms, which are out of the scope of this report.
|
||
|
||
6. Conclusion
|
||
|
||
This report proposes robust low-cost and real-time position and attitude estimation algorithms based on ground-based beacons and magnetometers. Three sets of ULAs equipped on the object with distinguishable work frequencies receive signals transmitted from three ground-based beacons. The directions of the AOA vectors are estimated by fast 2-D DOA estimation algorithm. Based on the prior knowledge of initial attitude parameters, the position of the object is estimated based on a 3-D AOA location algorithm. Based on the position estimation results, current attitude parameters are updated and used to estimate the position parameters in the next estimation cycle. A recursive median filter is applied to remove large deviations from the estimation results and improve the performance. Simulations show that the proposed low-cost and real-time algorithms successfully estimate the position and attitude most of the time. The algorithms developed for this technique are applicable to the special case of a perfectly vertical magnetic field, thus additional angular transforms would need to be introduced for solutions to more general magnetic orientation cases.
|
||
|
||
21
|
||
|
||
7. References
|
||
1. Lee, T.; Sanyal, A.; Leok, M.; McClamroch, N. H. Deterministic Global Attitude Estimation. Proceedings of the 45th IEEE Conference of Decision and Control, San Diego, CA, December 2006; pp 3174–3179.
|
||
2. Siouris, G. Missile Guidance and Control; Springer: New York, 2004. 3. Etkins, B. Dynamics of Atmospheric Fight; Dover Publications: Mineola, NY, 1972. 4. Rappaport, T. S.; Reed, J. H.; Woerner, B. D. Position Location Using Wireless
|
||
Communications on Highways of the Future. IEEE Comm. Magazine 1996, 34 (10), 33–41. 5. Fang, B. T. Simple Solutions for Hyperbolic and Related Position Fixes. IEEE Trans. Aerospace Elect. Sys. 1990, 26 (5), 748–753. 6. Smith, O. J.; Abel, J. S. The Spherical Interpolation Method of Source Localization. IEEE J. Oceanic Eng. 1987, OE-12 (1), 246–252. 7. McGillem, C. D.; Rappaport, T. S. A Beacon Navigation Method for Autonomous Vehicles. IEEE Trans. Vehic. Tech. 1989, 38 (3), 132–139. 8. Raner, W. H.; Schmidt, M. O. Fundamentals of Surveying; Van Nostrand: New York, 1969. 9. Schell, S. V.; Gardner, W. A. Handbook of Statistics; Elsevier: New York, 1993; Vol. 10, pp 755–817. 10. Haykin, S., Ed. Array Signal Processing; Prentice-Hall: Englewood Cliffs, NJ, 1985. 11. Krim, H.; Viberg, M. Two Decades of Array Signal Processing Reserch: The Parametric Approach. IEEE Signal Process. Magazine 1996, 13, 67–94. 12. Schmidt, R. Multiple Emitter Location and Signal Parameter Estimation. IEEE Trans. Antennas Propagation 1986, 34 (3), 276–280. 13. Bienvenu, G.; Kopp, L. Principle de la Gonionmetrie Passive Adaptive. Proceedings 7’eme Colloque GRESIT, Nice, France, 1979; pp 106/1–106/10. 14. Roy, R.; Paulraj, A.; Kailath, T. Estimation of Signal Parameters via Rotational Invariance Techniques-Esprit. Proceedings IEEE ICASSP, Tokyo, Japan, 1986; Vol. 4, pp 2495–2498. 15. Roy, R.; Kailath, T. Esprit-Estimation of Signal Parameters via Rotational Invariance Techniques. Opt. Eng. 1990, 29 (4), 296–313.
|
||
22
|
||
|
||
16. Wu, Y.; Liao, G.; So, H. C. A Fast Algorithm for 2-D Direction-of-Arrival Estimation. Signal Processing 2003, 83 (8), 1827–1831.
|
||
17. Tayem, N.; Kwon, H. M. Azimuth and Elevation Angle Estimation With No Failure and No Eigen Decomposition. Signal Processing 2006, 86 (1), 8–16.
|
||
18. Egziabher, D. G.; Elkaim, G. H. Mav Attitude Determination by Vector Matching. IEEE Trans. Aerospace Elect. Sys. 2008, 44 (3), 1012–1028.
|
||
19. Lerner, G. M. Spacecraft Attitude Determination and Control; D. Reidel Publishing Co.: Dordrecht, The Netherlands, 1978; Ch. 3, pp 420–428.
|
||
20. Wahba, G. A Least Squares Estimate of Spacecraft Attitude. SIAM Review 1965, 7 (3), 409.
|
||
21. Black, H. D. A Passive System for Determining the Attitude of a Satellite. AIAA Journal 1964, 2 (7), 1350–1351.
|
||
22. Carta, D. G.; Lackowski, D. H. Estimation of Orthogonal Transformations in Strapdown Inertial Systems. IEEE Transactions on Automatic Control 1972, AC-17, 97–100.
|
||
23. Bar-Itzhack, I. Y.; Reiner, J. Recursive Attitude Determination From Vector Observations: Direction Cosine Matrix Identification. Journal of Guidance, Control and Dynamics 1984, 7 (1), 51–56.
|
||
24. Ickes, B. P. A New Method for Performing Digital Control System Attitude Computations Using Quaternions. AIAA Journal 1970, 8 (1), 13–17.
|
||
25. Shuster, M. D.; Oh, S. D. Three-Axis Attitude Determination From Vector Observations. Journal of Guidance and Control 1981, 4 (1), 70–77.
|
||
26. Bar-Itzhack, I. Y.; Oshman, Y. Attitude Determination From Vector Observation: Quaternion Estimation. IEEE Trans. Aerospace Elect. Sys. 1985, AES-21 (1), 128–136.
|
||
27. Choukroun, D.; Bar-Itzhack, I. Y.; Oshman, Y. Novel Quaternion Kalman Flter. IEEE Trans. Aerospace and Elect. Sys. 2006, 42 (1), 174–190.
|
||
28. Bar-Itzhack, I. Y.; Idan, M. Recursive Attitude Determination From Vector Observation. J. Guidance, Control, and Dynamics 1987, 10 (2), 152–157.
|
||
29. Wilson, M. Attitude Determination With Magnetometers for Gun-Launched Munitions; ARL-TR-3209; U.S. Army Research Laboratory: Aberdeen Proving Ground, MD, August 2004.
|
||
30. Wilson, M. J. Nonlinear Projectile Attitude Estimation With Magnetometers and Angular Rate Sensors; Master’s Thesis, University of Delaware, Newark, DE, August 2006.
|
||
23
|
||
|
||
31. Wilson, M. J. Projectile Navigation and the Application to Magnetometers, 1st ed.; Ph.D. Thesis, University of Delaware, Newark, DE, August 2007.
|
||
32. Arce, G. R. Nonlinear Signal Processing; John Wiley and Sons, Inc.: Hoboken, NJ, 2005.
|
||
24
|
||
|
||
Appendix. Fast Two-Dimensional Direction of Arrival Estimation Algorithm
|
||
Consider one impinging signal represented by the vector s that is received by a set of parallel uniform linear arrays (ULAs) equipped on the object, as shown in figure A-1, and assume that the position of the object is known.
|
||
|
||
Figure A-1. One impinging signal is received by the system of three parallel ULAs equipped on the object.
|
||
|
||
When the fast two-dimensional direction of arrival estimation algorithm is used, the elevation
|
||
|
||
angle and the azimuth angle are estimated as
|
||
cosine theorem, the included angle between the Xb
|
||
|
||
axˆisananddˆs,
|
||
|
||
respectively. is
|
||
|
||
According to the
|
||
|
||
x cos1 (cos cos(90 )) cos1 (cos sin ).
|
||
|
||
(A-1)
|
||
|
||
bTaxehaiescdooinfreascntcdioatnnheobfeobdsjeeitcnetr.tmhBeinaEesdaer,dtohon-nfwixthheedossceiogsonurardlfiadnciareteeicntcifaoinnnibtese,
|
||
|
||
determined by the positions and the included angle , a body-fixed coordinates can
|
||
|
||
of the cone with an be found to
|
||
|
||
satisfy the estimated elevation and azimuth angles. Figure A-2 shows two sets of body-fixed
|
||
|
||
coordinates ( X b ,
|
||
|
||
Yb ,
|
||
|
||
Z
|
||
|
||
b
|
||
|
||
)
|
||
|
||
and
|
||
|
||
(
|
||
|
||
X
|
||
|
||
' b
|
||
|
||
,
|
||
|
||
Yb' ,
|
||
|
||
Z
|
||
|
||
' b
|
||
|
||
)
|
||
|
||
satisfying
|
||
|
||
the
|
||
|
||
given
|
||
|
||
conditions.
|
||
|
||
Therefore, given
|
||
|
||
the position of the object, one angle of arrival (AOA) vector is not sufficient to determine the
|
||
|
||
attitude of the object.
|
||
|
||
25
|
||
|
||
Figure A-2. One impinging signal determines a cone, on whose surface infinite body-fixed coordinates can be found to satisfy the estimated elevation and azimuth angles.
|
||
|
||
If of
|
||
|
||
two impinging signals are transmitted with direction vectors s1 and s2 are determined by the included angle between the
|
||
|
||
of Xb
|
||
|
||
s1 and s2 , two cones with axes axis and the vector directions.
|
||
|
||
Let the centers of the undersurfaces of the two cones be O1 and O2. There are at most two
|
||
|
||
intersection lines between the two cones, OA and OB , as shown in figure A-3. Let the
|
||
|
||
estimated elevation and azimuth angles from the first impinging signal be (ˆ1 , ˆ1 ) and the estimated elevation and azimuth angles from the second impinging signal be (ˆ 2 , ˆ2 ). In figure
|
||
|
||
A-3, assume the elevation
|
||
|
||
that and
|
||
|
||
the body-fixed coordinate ( X b , azimuth angles (ˆ1 , ˆ1 ) and (ˆ
|
||
|
||
Yb 2,
|
||
|
||
, Zb ˆ2 )
|
||
|
||
) on with
|
||
|
||
the intersection line respect to s1 and s2
|
||
|
||
OA satisfies , respectively.
|
||
|
||
On the intersection line OB , azimuth angles (ˆ1 , ˆ1 ) with elevation and azimuth angles
|
||
|
||
b(reoˆsd2py,e-cfˆtix2t)eodwsci1to.hoBrredosidpnyea-cteftixt(oeXdb'sc,2oY,owbr' ,dhZienb'ra)etesXa(tbi'Xsfob'i've, esYrtblh'a' ,epsZelb'Xe' )vb's'a.atitToisnhfiuaesns,dtohnely
|
||
|
||
(
|
||
|
||
X b , Yb , Zb ) satisfies all of the given conditions. In conclusion, given the position of the object,
|
||
|
||
two linearly independent AOA vectors are sufficient to determine the attitude of the object.
|
||
|
||
26
|
||
|
||
Figure A-3. Two impinging signals determine a pair of attitudes of the object.
|
||
27
|
||
|
||
NO. OF COPIES ORGANIZATION
|
||
|
||
1 (PDF only)
|
||
|
||
DEFENSE TECHNICAL INFORMATION CTR DTIC OCA 8725 JOHN J KINGMAN RD STE 0944 FORT BELVOIR VA 22060-6218
|
||
|
||
1 DIRECTOR US ARMY RESEARCH LAB IMNE ALC HRR 2800 POWDER MILL RD ADELPHI MD 20783-1197
|
||
|
||
1 DIRECTOR US ARMY RESEARCH LAB RDRL CIM L 2800 POWDER MILL RD ADELPHI MD 20783-1197
|
||
|
||
1 DIRECTOR US ARMY RESEARCH LAB RDRL CIM P 2800 POWDER MILL RD ADELPHI MD 20783-1197
|
||
|
||
1 DIRECTOR US ARMY RESEARCH LAB RDRL D 2800 POWDER MILL RD ADELPHI MD 20783-1197
|
||
|
||
ABERDEEN PROVING GROUND
|
||
1 DIR USARL RDRL CIM G (BLDG 4600)
|
||
|
||
28
|
||
|
||
NO. OF COPIES ORGANIZATION
|
||
1 CDR US ARMY TACOM ARDEC AMSRD AAR AEP M CILLI BLDG 382 PICATINNY ARSENAL NJ 07806-5000
|
||
1 US ARMY TARDEC AMSRD TAR R J WHITE MS 263 WARREN MI 48397-5000
|
||
2 COMMANDER US ARMY TACOM ARDEC AMSRD AAR AEP E D CARLUCCI M HOLLIS BLDG 94 PICATINNY ARSENAL NJ 07806-5000
|
||
1 COMMANDER US ARMY TACOM ARDEC AMSRD AR AEP S R WERKO BLDG 94 PICATINNY ARSENAL NJ 07806-5000
|
||
1 COMMANDER US ARMY TACOM ARDEC AMSRD AAR AEM C P MAGNOTTI BLDG 61S PICATINNY ARSENAL NJ 07806-5000
|
||
1 PM CAS SFAE AMO CAS P MANZ BLDG 172 PICATINNY ARSENAL NJ 07806-5000
|
||
ABERDEEN PROVING GROUND
|
||
37 DIR USARL RDRL WM J SMITH P PLOSTINS RDRL WML M ZOLTOSKI J NEWILL T VONG RDRL WML A B OBERLE R PEARSON A THOMPSON
|
||
|
||
NO. OF COPIES ORGANIZATION
|
||
RDRL WML D R BEYER M NUSCA P CONROY
|
||
RDRL WML E B GUIDOS P WEINACHT F FRESCONI J GARNER J SAHU
|
||
RDRL WML F E BUKOWSKI B DAVIS K HUBBARD T HARKINS D HEPNER M ILG G KATULKA (5 CPS) D LYON P MULLER B TOPPER P PEREGINO R HALL D GRZYBOWSKI D PETRICK
|
||
RDRL WML G T BROWN
|
||
RDRL WML H B SORENSEN C CANDLAND
|
||
|
||
29
|
||
|
||
INTENTIONALLY LEFT BLANK. 30
|
||
|