A Novel Method for Target Navigation and Mapping Based on Laser Ranging and MEMS/GPS Navigation

Making the sensor rigidly mounted in the target is the common characteristic of conventional navigation system. However, it is difficult or impossible to realize that for special applications such as the positioning of hostile aircraft. A novel new algorithm for target navigation and mapping is designed based on the position, attitude, and ranging information provided by laser distance detector (electronic distance measuring, LDS) and MEMS/GPS navigation, which can solve problem of the target navigation and mapping without any sensor in the target. The detailed error analysis shows that attitude error of MEMS/GPS is the main error source which dominated the accuracy of the algorithm. Based on the error analysis, a calibration algorithm is designed so as to improve the accuracy to a large extent. The result shows that, by using this new algorithm, the performance of target positioning can be efficiently improved, and the positioning error is less than 2 meters for the target within 1 kilometer range.


Introduction
Motion state of the target can be described by its accelerometer, velocity, position, and attitude.Navigation system is the most convenient device to provide these information by means of optical navigation, electronics navigation, dynamics navigation, acoustic navigation, and so on [1].For the mentioned navigation system, there is a common characteristic that the navigation sensor must be mounted in the target.For example, there must be optical observation device in the target which can measure the attitude vector from the target to another known point in terrestrial navigation and celestial navigation [2].MEMS inertial navigation system must rigidly mount the gyros and accelerometers to the target so as to measure its linear and angular movement.In communication domain, the target position method based on the cell site is also realized by communication between the cell phone and the base station [3].
Sometimes, conventional navigation system cannot act the right function in special application.For the positioning of those hard to or unable to reach places, it is difficult or expensive to mount the sensor in the target.For the positing of the enemy target in military, it is impossible [4,5].
Optical measure device in mapping is a good choice for the target mapping because it is unnecessary to mount the sensor in the target.On the basis of the known self-position, the target position can be measured accurately by means of angle measurement [6].For example, the total station can output precise slope distance from the instrument to a particular point by a theodolite integrated with an electronic distance meter (EDM).In a total station, the theodolite is used to measure angles in the horizontal and vertical planes, the distance between the instrument and the point is measured by the EDM.But for the mapping and navigation of the moving targets, optical measure device is unsuitable because it can only work in a relative static state [7].
MEMS/GPS integrated navigation system has the advantages of small size, light weight, and low cost; it can be applied in many navigation fields such as unmanned aircrafts, land vehicles, and robots [8].If the MEMS/GPS navigation system is allowed to substitute the optical measure device, the framework of the MEMS gyros constituted can be used to describe the vector from the instrument to the target.This can make the instrument suitable for static and moving target after being integrated with an EDM.In this paper, a new method based on MEMS/GPS and LDS is designed to realize the mapping and navigation of static and moving target.But due to the low accuracy of MEMS/GPS in attitude, the attitude error probably becomes the most important error source [9].If there is not suitable calibration method, the mapping and navigation result for the target will be severely influenced by the attitude error.Based on the detailed error analysis, calibration algorithm for the attitude error is designed so as to compensate the heading and pitch error of MEMS/GPS.The performance of the designed algorithm is evaluated by simulations and the results show that the new method exhibits excellent navigation and mapping performance.

Positioning Algorithm of the Target
2.1.Basic Definition for the Coordinate Systems.For the convenience of the observer and the target description, two coordinate systems should be defined.The first coordinate system is the Earth-fixed coordinate system (        ).It is a geocentric coordinate system which is rigidly bound to the Earth (see Figure 1).Its center (  ) is located at the geometric center of the Earth, the     axis passes through the zero longitude in the equator plane, the     axis is directed to the North Pole, and the     axis completes the right-handed orthogonal triad system.The other coordinate system is geographic coordinate system (      ; see Figure 1) whose center is located in the observer, the   axis is horizontal-east, the   axis is horizontal-north, and the   axis completes the right-handed orthogonal triad system.
The relationship between         and       can be described by the cosine transform matrix (   ) [9]: where   and   are, respectively, the latitude and longitude of point  in         .

Basic
From (3), it can be seen that the calculation of oT becomes the key for position calculation of the target  on the basis of the known position of observer .This is also the first research activity.

Positioning Calculation for the Target T.
According to the laws of Euler angle rotation, one orthogonal coordinate system can be transformed into another orthogonal coordinate system by three rotations.For example, three times rotation of , , and  can realize the transform from the geographic coordinate system to the body coordinate system, where , , and  are, respectively, the angle of heading, pitch, and roll (see Figure 2).
From Figure 2, it can obviously be found that the vector of oT is tightly concerned with the ranging between  and  and the attitude of oT in       .By involving the unit vector of e 1 , e 2 , e 3 , the vector of oT can also be described in       .That is, where    ,    ,    is the coordinate difference between  and  in       .
In order to calculate the vector of oT, assume a new vector of oC in       : where  is the distance between  and .From ( 5), the vector of oC has, in fact, the same direction as   axis.Its mold is equivalent to the ranging between  and .
For the transform between oC and oT, two rotations of  and  can accomplish the mission, because the final rotation of  will not change the direction of oT.In Figure 2, it can be seen that the   axis has the same direction as the   axis.
The rotation of  and  can be described by the following matrix transform.The first rotation transform matrix of  angle is The second rotation transform matrix of  angle is According to the definition of oT and oC, the vector of oT can be expressed based on the former transform matrix: Involving ( 6) and ( 7) into (8), the final calculation of oT can be got as follows: For the vector of oT, its description in (2) and ( 4) is the same vector.On the other hand, the unit vector of i 1 , i 2 , i 3 and the unit vector of e 1 , e 2 , e 3 have the following relationship: According to the relationship of ( 10), the coordinate difference between  and  in         can be got: Thus, the position of the target (   ,    ,    ) can be got based on (3) and ( 11): All the variables in (12) are ideal.In order to calculate the target position, the position of o, the distance between  and , and the orientation of oT must be known by suitable measurement devices.
If the MEMS/GPS can be rigidly mounted in the observer along with the body coordinate system, the position of  can be calculated from the position output of MEMS/GPS and the orientation of oT can be described by the attitude of MEMS/GPS.On the other hand, LDS can measure the distance if it is mounted along with the   axis of the body coordinate system accurately.
After getting the measurement information of MEMS/ GPS and LDS, the Cartesian coordinates of observer  can be calculated according to the following equation [10]: where   = /√1 −  2 sin 2 φ ,  is the long axle of the Earth, and  is the eccentricity of the Earth.By substituting the ideal variables with the measurement result, the final of Cartesian coordinates of the target can be got: Because MEMS/GPS and LDS can realize real-time measurement, the new algorithm can be used for both the static and the moving target.Besides the target position, the new algorithm can also measure many other pieces of information including longitude, latitude, height of the target, the slant range from one point to point, and the height difference and horizontal ranging between two targets.These pieces of information can all be calculated based on the position information of the target.
For the slant range ( D) from one point to another point, D can be calculated by the following equation: where

Error Analysis for the Algorithm
As mentioned in the former introduction, MEMS/GPS has a relatively low accuracy of attitude.For example, NAV440 GPS-aided MEMS inertial system has a 0.
The following result can be got by using the same method: For the distance which is minor to 1 kilometer, the relationship between  and radius of the Earth will satisfy where  is the radius of the Earth. Then, That is to say, Thus, where Δ 1 is the positioning error caused by the positioning error of MEMS/GPS.

Positioning Error Caused by the Error of LDS.
Based on (12) and ( 14), the target positioning error caused by the error of LDS can be calculated with the neglect of other error resources: Because Δ and Δ are all small error angles, then Based on the condition of (28), the error can be got as follows: As shown in Figure 3(a), the positioning error of target  which is caused by the positioning error of MEMS/GPS is mainly concerned with Δ 11 , because the calculation of √ 0.2 2 + 0.2 2 is 0.2828, and the error is almost independent of the parameter .The positioning error of the target in Figure 3(b) will reach 1.7 m when the parameter  is about 1 kilometer.The main reason is the linear error of LDS besides the error of 0.28 m stimulated by condition 1.It also can be seen that the positioning error of the target added quickly after the attitude error is involved in Figure 3(c).In the meantime, the error caused by Δ and Δ is linear to the parameter .

Calibration for the Algorithm
For navigation and mapping with higher accuracy demand, the positioning algorithm probably cannot meet the demands because of the measure error.Former simulation has explained the reason which is because of errors of MEMS/ GPS and LDS.Among all errors, attitude error is the most important because it dominates the accuracy of the target position.That is to say, the positioning error of point  and the ranging error of  can be neglected when the attitude error of MEMS/GPS is bigger than 0.1 ∘ , especially when the distance  is minor to 500 m.
Besides the attitude error mentioned, the attitude accuracy of MEMS/GPS will decline because the gyro scale factor and the misalignment parameters will change with time [15].These parameters can only be calibrated in laboratory.Thus, a new calibration method for the system should be designed to improve and maintain the performance of the system.
Because error of Δ   , Δ   , Δ   , and Δ can be neglected when the parameter  is minor to 500 m, The following equation can be got: In (34), most variables can be measured by MEMS/GPS and LDS except the ideal attitude angle of  and .That is to say, angle of  and  can be calculated so as to replace φ and H.But (34) is the type of transcendental equation which is very difficult to solve.In order to get the ideal attitude, (34) should be converted into linear equation which can make the solution easy.
The parameters φ and H can be substituted as Involving ( 35) into (34) and using the same simplification as (28), the transcendental equation can be converted into linear equation: (36) Equation (36) can also be described as Three equations are redundant for the solution of two unknown variables.Assuming C = C   C 1 and involving C  , Δ and Δ can be estimated by the least-square method [16]: Then, φ = φ−Δ φ and Ĥ = H−Δ Ĥ can substitute φ and H.The calibration can improve the attitude of the MEMS/GPS, so as to get the target position with higher accuracy.As shown in Figure 4(a), the calibration algorithm can estimate the angle of Δ and Δ, the residual errors of Δ and Δ are all minor to 0.02 ∘ ; this is a very high accuracy of attitude for the target navigation and mapping.After involving errors of MEMS/GPS and LDS, the accuracy of the calibration decreased because of the error sources.The residual error of Δ is about 0.07 ∘ , and the residual error of Δ is about 0.01 ∘ in Figure 4(b).But with the increasing of   , the residual error of Δ will decrease to 00.04 ∘ and that of Δ to 0.005 ∘ .That is to say, the performance of the calibration will increase if there is a relatively long distance of   .If the type of the angle error is changed, the calibration has the same performance.The comparison of ideal and estimated heading error in Figures 4(d), 4(e), and 4(f) can demonstrate it.

Simulation for the Target Positioning after Calibration.
Former simulations demonstrate that the calibration algorithm can estimate most attitude errors of MEMS/GPS.This will be very helpful for the accuracy improvement of the target position.The following simulation gave the final error analysis for the target position after the attitude error of MEMS/GPS is compensated.In order to compare the positioning performance to the uncalibrated algorithm, all errors of MEMS/GPS are the same as the former during the simulation except the error source demanded in the condition.As shown in Figures 5(a) and 5(b), the positioning error has been decreased to a large extent after the attitude error of MEMS/GPS is compensated (curve of Cal).For more information in Figures 5(a) and 5(b), Δ φ and Δ Ĥ are in fact the equivalent estimation of attitude error which will be influenced by the position error of  and ranging error of .Because the positioning error of the target is also decreased when the distance is zero, this means that the position error of MEMS/GPS is partly compensated by the calibration algorithm.On the other hand, error of condition 2 is bigger than error of condition 3 because there is more residual error when

Figure 2 :
Figure 2: Transform between two orthogonal coordinate systems.

Figure 3 :
Figure 3: Simulation of error analysis caused by MEMS/GPS and LDS.

4. 1 .
Description for the Known Reference Point.With the condition of another point () nearby the observer, its position can be got based on the former algorithm.That is,

𝑑 4 . 2 .
hand, the ideal position of  can be described based on (12):  sin    cos  cos  −  sin  cos  ] Attitude Calculation Based on the Known Reference Point.If the position of the reference can be got in advance by means of MEMS/GPS, the substraction of (32) and (33) can construct a new measurement variable which can be used to calculated the parameter  and .That is, (sin H − sin )   (cos φ cos H − cos  cos ) −  (sin φ cos H − sin  cos ) cos HΔ −  sin φ cos HΔ −   cos φ sin HΔ −  cos φ cos HΔ +   sin φ sin HΔ ] ] ] .

e o, o e T, and oT in
Description for the Vectors.For vectors of o         , all of them can be described if the unit vector (i 1 , i 2 , i 3 ) in         is involved.Thus, vectors of o

the Cartesian coordinates of observer 𝑜 in 𝑜 𝑒 𝑥 𝑒 𝑦 𝑒 𝑧 𝑒 ; (𝑥 𝑒 𝑇 , 𝑦 𝑒 𝑇 , 𝑧 𝑒 𝑇 ) is the Cartesian coordinates of target 𝑇 in 𝑜 𝑒 𝑥 𝑒 𝑦 𝑒 𝑧 𝑒 ; 𝑥 𝑒 𝑜𝑇 , 𝑦 𝑒 𝑜𝑇 , 𝑧 𝑒 𝑜𝑇 is the coordinate difference between 𝑇 and 𝑜 in 𝑜 𝑒 𝑥 𝑒 𝑦 𝑒 𝑧 𝑒 .
) are, respectively, the measurement result of the Cartesian coordinates for targets  1 and  2 .For the height difference (Δ H) between the two targets, x 1 , ỹ 1 , z 1 ) and ( x 2 , ỹ 2 , z 2 4 ∘ (RMS) accuracy of pitch and roll and a 1.0 ∘ (RMS) accuracy of heading for all motion[11].ADIS16480 MEMS/GPS has an accuracy of 0.1 ∘ (pitch, roll) and 0.3 ∘ (heading) for static state[12].Besides the attitude error, the positioning error of MEMS/GPS probably has a severe impact on the position calculation of the target.Comparing with the item of Δ 11 , another item of Δ 12 can be neglected because the error of the transform matrix is very small.For example, the first line of ΔC   has the following characteristic:Δ 11 = −sin   Δ  ≤     Δ      Δ 12 = sin   sin   Δ  − cos   cos   Δ  sin   cos   Δ  + cos   sin   Δ 3.1.PositioningErrorCaused by the Positioning Error of MEMS/GPS.The positioning error of MEMS/GPS will cause two errors which are tightly concerned with the calculation of the target.Those are the position error of point  and the cosine transform matrix of C  .Based on (12) and (14), the target positioning error caused by the positioning error 11  sin  + Δ 12  cos  cos  + Δ 13 (− sin  cos ) 21  sin  + Δ 22  cos  cos  + Δ 23 (− sin  cos ) cos  cos  − Δ sin  cos  Δ sin  sin  − Δ cos  cos  Simulation for the Positioning Error of the Target.Assume that the initial position of the observer is east longitude 126 ∘ and north latitude 45 ∘ .The longitude and latitude error of MEMS/GPS is 0.2 m and the heading and pitch error of MEMS/GPS are, respectively, 0.2 ∘ and 0.1 ∘ [12].LDS has a linearity error of 0.15% of the range [14].Corresponding to three conditions, three simulation results are given in Figure3.By the way, the heading and pitch are in swaying state in order to follow a moving target:  = 30 ∘ + 7 ∘ sin(2 * /7) and  = 2 ∘ + 1 ∘ sin(2 * /7).Condition 1.Only the position error of MEMS/GPS is involved.Condition 2. The position error of MEMS/GPS and the linearity error of LDS are involved.Condition 3.All errors of MEMS/GPS and LDS are involved.