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

Authors: Jianhua Cheng; Rene Landry; Daidai Chen; Dongxue Guan
Journal: Journal of Applied Mathematics
 (2014)

---

## Abstract

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.


---

## Body

## 1. 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.


## 2. 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(oexeyeze). It is a geocentric coordinate system which is rigidly bound to the Earth (see Figure 1). Its center (oe) is located at the geometric center of the Earth, the oexe axis passes through the zero longitude in the equator plane, the oeze axis is directed to the North Pole, and the oeye axis completes the right-handed orthogonal triad system. The other coordinate system is geographic coordinate system (oxtytzt; see Figure 1) whose center is located in the observer, the oxt axis is horizontal-east, the oyt axis is horizontal-north, and the ozt axis completes the right-handed orthogonal triad system.Figure 1
Definition for the coordinate systems.The relationship betweenoexeyeze and oxtytzt can be described by the cosine transform matrix (Cte) [9]:
(1)Cte=[cos⁡⁡λo-sin⁡λo0sin⁡λocos⁡⁡λo0001]·[1000cos⁡⁡φo-sin⁡φo0sin⁡φocos⁡⁡φo],
where φo and λo are, respectively, the latitude and longitude of point o in oexeyeze.


### 2.2. Basic Description for the Vectors

For vectors ofoeo, oeT, and oT in oexeyeze, all of them can be described if the unit vector (i1,i2,i3) in oexeyeze is involved. Thus, vectors of oeo, oeT, and oT can be described as
(2)oeo=xoei1+yoei2+zoei3oeT=xTei1+yTei2+zTei3oT=xoTei1+yoTei2+zoTei3,
where (xoe,yoe,zoe) is the Cartesian coordinates of observer o in oexeyeze; (xTe,yTe,zTe) is the Cartesian coordinates of target T in oexeyeze; xoTe, yoTe, zoTe is the coordinate difference between T and o in oexeyeze.The three vectors in (2) have the following relationship:
(3)oeT=oeo+oT.From (3), it can be seen that the calculation of oT becomes the key for position calculation of the target T on the basis of the known position of observer o. This is also the first research activity.


### 2.3. 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 ofH, ϕ, and θ can realize the transform from the geographic coordinate system to the body coordinate system, where H, ϕ, and θ are, respectively, the angle of heading, pitch, and roll (see Figure 2).Figure 2
Transform between two orthogonal coordinate systems.From Figure2, it can obviously be found that the vector of oT is tightly concerned with the ranging between o and T and the attitude of oT in oxtytzt. By involving the unit vector of e1, e2, e3, the vector of oT can also be described in oxtytzt. That is,
(4)oT=xoTte1+yoTte2+yoTte3,
where xoTt,yoTt,zoTt is the coordinate difference between T and o in oxtytzt.In order to calculate the vector ofoT, assume a new vector of oC in oxtytzt:
(5)oC=0e1+de2+0e3,
where d is the distance between o and T.From (5), the vector of oC has, in fact, the same direction as oyt axis. Its mold is equivalent to the ranging between o and T.For the transform betweenoC and oT, two rotations of H 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 oyt axis has the same direction as the oyb axis.The rotation ofH and ϕ can be described by the following matrix transform. The first rotation transform matrix of H angle is
(6)Ct1=[cos⁡⁡Hsin⁡H0-sin⁡Hcos⁡⁡H0001].The second rotation transform matrix ofϕ angle is
(7)C12=[1000cos⁡⁡ϕsin⁡ϕ0-sin⁡ϕcos⁡⁡ϕ].According to the definition ofoT and oC, the vector of oT can be expressed based on the former transform matrix:
(8)oT=C12Ct1oC.Involving (6) and (7) into (8), the final calculation of oT can be got as follows:
(9)[xoTtyoTtzoTt]=[1000cos⁡⁡ϕsin⁡ϕ0-sin⁡ϕcos⁡⁡ϕ]·[cos⁡⁡Hsin⁡H0-sin⁡Hcos⁡⁡H0001][0d0]=[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H].For the vector ofoT, its description in (2) and (4) is the same vector. On the other hand, the unit vector of i1, i2, i3 and the unit vector of e1, e2, e3 have the following relationship:
(10)[i1i2i3]=Cei[e1e2e3].According to the relationship of (10), the coordinate difference between T and o in oexeyeze can be got:
(11)[xoTeyoTezoTe]=Cte[xoTtyoTtzoTt].Thus, the position of the target(xTe,yTe,zTe) can be got based on (3) and (11):
(12)[xTeyTezTe]=[xAeyAezAe]+Cte[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H].All the variables in (12) are ideal. In order to calculate the target position, the position of o, the distance between o and T, 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 ofo 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 oyb axis of the body coordinate system accurately.Thus, define the new variables of the measurement:(1)
latitude, longitude, and height measurement output of MEMS/GPS:φ~o, λ~o, h~o;(2)
heading, pitch, and roll measurement output of MEMS/GPS:H~, ϕ~, θ~;(3)
ranging measurement output of LDS:d~.After getting the measurement information of MEMS/GPS and LDS, the Cartesian coordinates of observero can be calculated according to the following equation [10]:
(13)x~Ae=(RN+h~A)cos⁡⁡φ~Acos⁡⁡λ~Ay~Ae=(RN+h~A)cos⁡⁡φ~Asin⁡λ~Az~Ae=[RN(1-e2)+h~A]sin⁡φ~A,
where RN=a/1-e2sin2φ~o, a is the long axle of the Earth, and e 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:(14)[x~Tey~Tez~Te]=[x~Aey~Aez~Ae]+[cos⁡⁡λ~A-sin⁡λ~Acos⁡⁡φ~Asin⁡λ~Acos⁡⁡φ~Asin⁡λ~Acos⁡⁡λ~Acos⁡⁡φ~A-cos⁡⁡λ~Asin⁡φ~A0sin⁡φ~Acos⁡⁡φ~A]·[d~sin⁡H~d~cos⁡⁡ϕ~cos⁡⁡H~-d~sin⁡ϕ~cos⁡⁡H~].


### 2.4. Functions for the Target Mapping and Navigation

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:
(15)D~=(x~T2e-x~T1e)2+(y~T2e-y~T1e)2+(z~T2e-z~T1e)2,
where (x~T1e,y~T1e,z~T1e) and (x~T2e,y~T2e,z~T2e) are, respectively, the measurement result of the Cartesian coordinates for targets T1 and T2.For the height difference (ΔH~) between the two targets,
(16)ΔH~=|(x~T2e)2+(y~T2e)2cos⁡⁡φ~T2-(x~T1e)2+(y~T1e)2cos⁡⁡φ~T1|.


## 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(oexeyeze). It is a geocentric coordinate system which is rigidly bound to the Earth (see Figure 1). Its center (oe) is located at the geometric center of the Earth, the oexe axis passes through the zero longitude in the equator plane, the oeze axis is directed to the North Pole, and the oeye axis completes the right-handed orthogonal triad system. The other coordinate system is geographic coordinate system (oxtytzt; see Figure 1) whose center is located in the observer, the oxt axis is horizontal-east, the oyt axis is horizontal-north, and the ozt axis completes the right-handed orthogonal triad system.Figure 1
Definition for the coordinate systems.The relationship betweenoexeyeze and oxtytzt can be described by the cosine transform matrix (Cte) [9]:
(1)Cte=[cos⁡⁡λo-sin⁡λo0sin⁡λocos⁡⁡λo0001]·[1000cos⁡⁡φo-sin⁡φo0sin⁡φocos⁡⁡φo],
where φo and λo are, respectively, the latitude and longitude of point o in oexeyeze.


## 2.2. Basic Description for the Vectors

For vectors ofoeo, oeT, and oT in oexeyeze, all of them can be described if the unit vector (i1,i2,i3) in oexeyeze is involved. Thus, vectors of oeo, oeT, and oT can be described as
(2)oeo=xoei1+yoei2+zoei3oeT=xTei1+yTei2+zTei3oT=xoTei1+yoTei2+zoTei3,
where (xoe,yoe,zoe) is the Cartesian coordinates of observer o in oexeyeze; (xTe,yTe,zTe) is the Cartesian coordinates of target T in oexeyeze; xoTe, yoTe, zoTe is the coordinate difference between T and o in oexeyeze.The three vectors in (2) have the following relationship:
(3)oeT=oeo+oT.From (3), it can be seen that the calculation of oT becomes the key for position calculation of the target T on the basis of the known position of observer o. This is also the first research activity.


## 2.3. 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 ofH, ϕ, and θ can realize the transform from the geographic coordinate system to the body coordinate system, where H, ϕ, and θ are, respectively, the angle of heading, pitch, and roll (see Figure 2).Figure 2
Transform between two orthogonal coordinate systems.From Figure2, it can obviously be found that the vector of oT is tightly concerned with the ranging between o and T and the attitude of oT in oxtytzt. By involving the unit vector of e1, e2, e3, the vector of oT can also be described in oxtytzt. That is,
(4)oT=xoTte1+yoTte2+yoTte3,
where xoTt,yoTt,zoTt is the coordinate difference between T and o in oxtytzt.In order to calculate the vector ofoT, assume a new vector of oC in oxtytzt:
(5)oC=0e1+de2+0e3,
where d is the distance between o and T.From (5), the vector of oC has, in fact, the same direction as oyt axis. Its mold is equivalent to the ranging between o and T.For the transform betweenoC and oT, two rotations of H 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 oyt axis has the same direction as the oyb axis.The rotation ofH and ϕ can be described by the following matrix transform. The first rotation transform matrix of H angle is
(6)Ct1=[cos⁡⁡Hsin⁡H0-sin⁡Hcos⁡⁡H0001].The second rotation transform matrix ofϕ angle is
(7)C12=[1000cos⁡⁡ϕsin⁡ϕ0-sin⁡ϕcos⁡⁡ϕ].According to the definition ofoT and oC, the vector of oT can be expressed based on the former transform matrix:
(8)oT=C12Ct1oC.Involving (6) and (7) into (8), the final calculation of oT can be got as follows:
(9)[xoTtyoTtzoTt]=[1000cos⁡⁡ϕsin⁡ϕ0-sin⁡ϕcos⁡⁡ϕ]·[cos⁡⁡Hsin⁡H0-sin⁡Hcos⁡⁡H0001][0d0]=[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H].For the vector ofoT, its description in (2) and (4) is the same vector. On the other hand, the unit vector of i1, i2, i3 and the unit vector of e1, e2, e3 have the following relationship:
(10)[i1i2i3]=Cei[e1e2e3].According to the relationship of (10), the coordinate difference between T and o in oexeyeze can be got:
(11)[xoTeyoTezoTe]=Cte[xoTtyoTtzoTt].Thus, the position of the target(xTe,yTe,zTe) can be got based on (3) and (11):
(12)[xTeyTezTe]=[xAeyAezAe]+Cte[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H].All the variables in (12) are ideal. In order to calculate the target position, the position of o, the distance between o and T, 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 ofo 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 oyb axis of the body coordinate system accurately.Thus, define the new variables of the measurement:(1)
latitude, longitude, and height measurement output of MEMS/GPS:φ~o, λ~o, h~o;(2)
heading, pitch, and roll measurement output of MEMS/GPS:H~, ϕ~, θ~;(3)
ranging measurement output of LDS:d~.After getting the measurement information of MEMS/GPS and LDS, the Cartesian coordinates of observero can be calculated according to the following equation [10]:
(13)x~Ae=(RN+h~A)cos⁡⁡φ~Acos⁡⁡λ~Ay~Ae=(RN+h~A)cos⁡⁡φ~Asin⁡λ~Az~Ae=[RN(1-e2)+h~A]sin⁡φ~A,
where RN=a/1-e2sin2φ~o, a is the long axle of the Earth, and e 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:(14)[x~Tey~Tez~Te]=[x~Aey~Aez~Ae]+[cos⁡⁡λ~A-sin⁡λ~Acos⁡⁡φ~Asin⁡λ~Acos⁡⁡φ~Asin⁡λ~Acos⁡⁡λ~Acos⁡⁡φ~A-cos⁡⁡λ~Asin⁡φ~A0sin⁡φ~Acos⁡⁡φ~A]·[d~sin⁡H~d~cos⁡⁡ϕ~cos⁡⁡H~-d~sin⁡ϕ~cos⁡⁡H~].


## 2.4. Functions for the Target Mapping and Navigation

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:
(15)D~=(x~T2e-x~T1e)2+(y~T2e-y~T1e)2+(z~T2e-z~T1e)2,
where (x~T1e,y~T1e,z~T1e) and (x~T2e,y~T2e,z~T2e) are, respectively, the measurement result of the Cartesian coordinates for targets T1 and T2.For the height difference (ΔH~) between the two targets,
(16)ΔH~=|(x~T2e)2+(y~T2e)2cos⁡⁡φ~T2-(x~T1e)2+(y~T1e)2cos⁡⁡φ~T1|.


## 3. 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.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. For example, Omni STAR HP of Trimble has accuracy of minor to 10 centimeters [13]. On the other hand, LDS also has measurement error. For example, the accuracy of AccuRange AR500 Laser Sensor is generally specified with a linearity of about +/− 0.15% of the range [14].


### 3.1. Positioning Error Caused 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 pointo and the cosine transform matrix of C~et. Based on (12) and (14), the target positioning error caused by the positioning error MEMS/GPS can be calculated with the neglect of other error resources:
(17)Δ1=Δ11+Δ12=[ΔxoeΔyoeΔzoe]+ΔCte[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H]=[ΔxoeΔyoeΔzoe]+[Δc11Δc12Δc13Δc21Δc22Δc23Δc31Δc32Δc33][dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H],
where Δxoe=x~oe-xoe, Δyoe=y~oe-yoe, Δzoe=z~oe-zoe, ΔCet=C~et-Cet.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 ΔCet has the following characteristic:
(18)Δc11=-sin⁡λoΔλo≤|Δλo|Δc12=sin⁡λosin⁡φoΔφA-cos⁡⁡λocos⁡⁡φoΔλo≤|Δφo|+|Δλo|Δc13=sin⁡λocos⁡⁡φoΔφo+cos⁡⁡λosin⁡φoΔλo≤|Δφo|+|Δλo|.Then,(19)Δc11dsin⁡H+Δc12dcos⁡⁡ϕcos⁡⁡H+Δc13(-dsin⁡ϕcos⁡⁡H)≤3d|Δλo|+2d|Δφo|.The following result can be got by using the same method:(20)Δc21dsin⁡H+Δc22dcos⁡⁡ϕcos⁡⁡H+Δc23(-dsin⁡ϕcos⁡⁡H)≤3d|Δλo|+2d|Δφo|Δc31dsin⁡H+Δc32dcos⁡⁡ϕcos⁡⁡H+Δc33(-dsin⁡ϕcos⁡⁡H)≤2d|Δφo|.For the distance which is minor to 1 kilometer, the relationship betweend and radius of the Earth will satisfy
(21)d≪R,
where R is the radius of the Earth.Then,(22)d2[(Δφo)2+(Δλo)2]≪R2[(Δφo)2+(Δλo)2]≈[(Δxoe)2+(Δyoe)2+(Δzoe)2].That is to say,(23)|Δ12|≪|Δ11|.Thus,(24)ΔP1=|Δ1|≈(Δxoe)2+(Δyoe)2+(Δzoe)2,
where ΔP1 is the positioning error caused by the positioning error of MEMS/GPS.


### 3.2. 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:
(25)Δ2=Cte[(d~-d)sin⁡H(d~-d)cos⁡⁡ϕcos⁡⁡H-(d~-d)sin⁡ϕcos⁡⁡H]=Cte[Δdsin⁡HΔdcos⁡⁡ϕcos⁡⁡H-Δdsin⁡ϕcos⁡⁡H].Thus,(26)ΔP2=|Δ2|=|Δd|.


### 3.3. Positioning Error Caused by the Attitude Error of MEMS/GPS

The target positioning error caused by the attitude error of MEMS/GPS can also be calculated with the neglect of other error resources:(27)Δ3=Cte[d(sin⁡H~-sin⁡H)d(cos⁡⁡ϕ~cos⁡⁡H~-cos⁡⁡ϕcos⁡⁡H)-d(sin⁡ϕ~cos⁡⁡H~-sin⁡ϕcos⁡⁡H)]=Cte[d[sin⁡(H+ΔH)-sin⁡H]d[cos⁡⁡(ϕ+Δϕ)cos⁡⁡(H+ΔH)-cos⁡⁡ϕcos⁡⁡H]-d[sin⁡(ϕ+Δϕ)cos⁡⁡(H+ΔH)-sin⁡ϕcos⁡⁡H]].BecauseΔϕ and ΔH are all small error angles, then
(28)sin⁡Δϕ≈Δϕcos⁡⁡ΔH≈1Δϕ·ΔH≪Δϕcos⁡⁡Δϕ≈1sin⁡ΔH≈ΔHΔϕ·ΔH≪ΔH.Based on the condition of (28), the error can be got as follows:
(29)Δ3=Cet[dΔHcos⁡⁡H-dΔHcos⁡⁡ϕcos⁡⁡H-dΔϕsin⁡ϕcos⁡⁡HdΔHsin⁡ϕsin⁡H-dΔϕcos⁡⁡ϕcos⁡⁡H](30)ΔP3=|Δ3|=d((ΔH)2(cos⁡2H+cos⁡2ϕ-cos⁡⁡2ϕsin2H)(Δϕ)2+(Δϕ)2cos⁡2H+ΔHΔϕsin⁡2ϕcos⁡⁡H×(Δϕ)2(cos⁡⁡H-sin⁡H))1/2.


### 3.4. 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 Figure 3. By the way, the heading and pitch are in swaying state in order to follow a moving target: H=30°+7°sin⁡(2π*t/7) and ϕ=2°+1°sin⁡(2π*t/7).Simulation of error analysis caused by MEMS/GPS and LDS.
(a)
Results of condition 1
(b)
Results of conditions 1 and 2
(c)
Results of conditions 1, 2, and 3Condition
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.As shown in Figure3(a), the positioning error of target T which is caused by the positioning error of MEMS/GPS is mainly concerned with Δ11, because the calculation of 0.22+0.22 is 0.2828, and the error is almost independent of the parameter d. The positioning error of the target in Figure 3(b) will reach 1.7 m when the parameter d 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 ΔH and Δϕ is linear to the parameter d.


## 3.1. Positioning Error Caused 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 pointo and the cosine transform matrix of C~et. Based on (12) and (14), the target positioning error caused by the positioning error MEMS/GPS can be calculated with the neglect of other error resources:
(17)Δ1=Δ11+Δ12=[ΔxoeΔyoeΔzoe]+ΔCte[dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H]=[ΔxoeΔyoeΔzoe]+[Δc11Δc12Δc13Δc21Δc22Δc23Δc31Δc32Δc33][dsin⁡Hdcos⁡⁡ϕcos⁡⁡H-dsin⁡ϕcos⁡⁡H],
where Δxoe=x~oe-xoe, Δyoe=y~oe-yoe, Δzoe=z~oe-zoe, ΔCet=C~et-Cet.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 ΔCet has the following characteristic:
(18)Δc11=-sin⁡λoΔλo≤|Δλo|Δc12=sin⁡λosin⁡φoΔφA-cos⁡⁡λocos⁡⁡φoΔλo≤|Δφo|+|Δλo|Δc13=sin⁡λocos⁡⁡φoΔφo+cos⁡⁡λosin⁡φoΔλo≤|Δφo|+|Δλo|.Then,(19)Δc11dsin⁡H+Δc12dcos⁡⁡ϕcos⁡⁡H+Δc13(-dsin⁡ϕcos⁡⁡H)≤3d|Δλo|+2d|Δφo|.The following result can be got by using the same method:(20)Δc21dsin⁡H+Δc22dcos⁡⁡ϕcos⁡⁡H+Δc23(-dsin⁡ϕcos⁡⁡H)≤3d|Δλo|+2d|Δφo|Δc31dsin⁡H+Δc32dcos⁡⁡ϕcos⁡⁡H+Δc33(-dsin⁡ϕcos⁡⁡H)≤2d|Δφo|.For the distance which is minor to 1 kilometer, the relationship betweend and radius of the Earth will satisfy
(21)d≪R,
where R is the radius of the Earth.Then,(22)d2[(Δφo)2+(Δλo)2]≪R2[(Δφo)2+(Δλo)2]≈[(Δxoe)2+(Δyoe)2+(Δzoe)2].That is to say,(23)|Δ12|≪|Δ11|.Thus,(24)ΔP1=|Δ1|≈(Δxoe)2+(Δyoe)2+(Δzoe)2,
where ΔP1 is the positioning error caused by the positioning error of MEMS/GPS.


## 3.2. 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:
(25)Δ2=Cte[(d~-d)sin⁡H(d~-d)cos⁡⁡ϕcos⁡⁡H-(d~-d)sin⁡ϕcos⁡⁡H]=Cte[Δdsin⁡HΔdcos⁡⁡ϕcos⁡⁡H-Δdsin⁡ϕcos⁡⁡H].Thus,(26)ΔP2=|Δ2|=|Δd|.


## 3.3. Positioning Error Caused by the Attitude Error of MEMS/GPS

The target positioning error caused by the attitude error of MEMS/GPS can also be calculated with the neglect of other error resources:(27)Δ3=Cte[d(sin⁡H~-sin⁡H)d(cos⁡⁡ϕ~cos⁡⁡H~-cos⁡⁡ϕcos⁡⁡H)-d(sin⁡ϕ~cos⁡⁡H~-sin⁡ϕcos⁡⁡H)]=Cte[d[sin⁡(H+ΔH)-sin⁡H]d[cos⁡⁡(ϕ+Δϕ)cos⁡⁡(H+ΔH)-cos⁡⁡ϕcos⁡⁡H]-d[sin⁡(ϕ+Δϕ)cos⁡⁡(H+ΔH)-sin⁡ϕcos⁡⁡H]].BecauseΔϕ and ΔH are all small error angles, then
(28)sin⁡Δϕ≈Δϕcos⁡⁡ΔH≈1Δϕ·ΔH≪Δϕcos⁡⁡Δϕ≈1sin⁡ΔH≈ΔHΔϕ·ΔH≪ΔH.Based on the condition of (28), the error can be got as follows:
(29)Δ3=Cet[dΔHcos⁡⁡H-dΔHcos⁡⁡ϕcos⁡⁡H-dΔϕsin⁡ϕcos⁡⁡HdΔHsin⁡ϕsin⁡H-dΔϕcos⁡⁡ϕcos⁡⁡H](30)ΔP3=|Δ3|=d((ΔH)2(cos⁡2H+cos⁡2ϕ-cos⁡⁡2ϕsin2H)(Δϕ)2+(Δϕ)2cos⁡2H+ΔHΔϕsin⁡2ϕcos⁡⁡H×(Δϕ)2(cos⁡⁡H-sin⁡H))1/2.


## 3.4. 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 Figure 3. By the way, the heading and pitch are in swaying state in order to follow a moving target: H=30°+7°sin⁡(2π*t/7) and ϕ=2°+1°sin⁡(2π*t/7).Simulation of error analysis caused by MEMS/GPS and LDS.
(a)
Results of condition 1
(b)
Results of conditions 1 and 2
(c)
Results of conditions 1, 2, and 3Condition
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.As shown in Figure3(a), the positioning error of target T which is caused by the positioning error of MEMS/GPS is mainly concerned with Δ11, because the calculation of 0.22+0.22 is 0.2828, and the error is almost independent of the parameter d. The positioning error of the target in Figure 3(b) will reach 1.7 m when the parameter d 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 ΔH and Δϕ is linear to the parameter d.


## 4. 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 pointo and the ranging error of d can be neglected when the attitude error of MEMS/GPS is bigger than 0.1°, especially when the distance d 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.


### 4.1. Description for the Known Reference Point

With the condition of another point(R) nearby the observer, its position can be got based on the former algorithm. That is,
(31)[x~Rey~Rez~Re]=[x~oey~oez~oe]+C~te[d~oRsin⁡H~d~oRcos⁡⁡ϕ~cos⁡⁡H~-d~oRsin⁡ϕ~cos⁡⁡H~].Because error ofΔxoe, Δyoe, Δzoe, and Δd can be neglected when the parameter d is minor to 500 m, The following equation can be got:
(32)[x~Rey~Rez~Re]=[xoeyoezoe]+Cte[doRsin⁡H~doRcos⁡⁡ϕ~cos⁡⁡H~-doRsin⁡ϕ~cos⁡⁡H~].On the other hand, the ideal position ofR can be described based on (12):
(33)[xReyRezRe]=[xoeyoezoe]+Cte[doRsin⁡HdoRcos⁡⁡ϕcos⁡⁡H-doRsin⁡ϕcos⁡⁡H].


### 4.2. 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 H and ϕ. That is,
(34)[ΔxReΔyReΔzRe]=[x~Re-xRey~Re-yRez~Re-zRe]=Cte[doR(sin⁡H~-sin⁡H)doR(cos⁡⁡ϕ~cos⁡⁡H~-cos⁡⁡ϕcos⁡⁡H)-doR(sin⁡ϕ~cos⁡⁡H~-sin⁡ϕcos⁡⁡H)].In (34), most variables can be measured by MEMS/GPS and LDS except the ideal attitude angle of ϕ and H. That is to say, angle of ϕ and H 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
(35)ϕ=ϕ~-ΔϕH=H~-ΔH.Involving (35) into (34) and using the same simplification as (28), the transcendental equation can be converted into linear equation:
(36)[ΔxReΔyReΔzRe]=Cte[doRcos⁡⁡H~ΔH-doRsin⁡ϕ~cos⁡⁡H~Δϕ-doRcos⁡⁡ϕ~sin⁡H~ΔH-doRcos⁡⁡ϕ~cos⁡⁡H~Δϕ+doRsin⁡ϕ~sin⁡H~ΔH].Equation (36) can also be described as
(37)[ΔxReΔyReΔzRe]=Cte[0doRcos⁡H~-doRsin⁡ϕ~cos⁡⁡H~-doRcos⁡⁡ϕ~sin⁡H~-doRcos⁡⁡ϕ~cos⁡⁡H~doRsin⁡ϕ~sin⁡H~][ΔϕΔH]=CteC1[ΔϕΔH].Three equations are redundant for the solution of two unknown variables. AssumingC=CteC1 and involving CT, Δϕ and ΔH can be estimated by the least-square method [16]:
(38)[Δϕ^ΔH^]=(CT·C)-1CT[ΔxReΔyReΔzRe].Then,ϕ^=ϕ~-Δϕ^ and H^=H~-Δ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.


## 4.1. Description for the Known Reference Point

With the condition of another point(R) nearby the observer, its position can be got based on the former algorithm. That is,
(31)[x~Rey~Rez~Re]=[x~oey~oez~oe]+C~te[d~oRsin⁡H~d~oRcos⁡⁡ϕ~cos⁡⁡H~-d~oRsin⁡ϕ~cos⁡⁡H~].Because error ofΔxoe, Δyoe, Δzoe, and Δd can be neglected when the parameter d is minor to 500 m, The following equation can be got:
(32)[x~Rey~Rez~Re]=[xoeyoezoe]+Cte[doRsin⁡H~doRcos⁡⁡ϕ~cos⁡⁡H~-doRsin⁡ϕ~cos⁡⁡H~].On the other hand, the ideal position ofR can be described based on (12):
(33)[xReyRezRe]=[xoeyoezoe]+Cte[doRsin⁡HdoRcos⁡⁡ϕcos⁡⁡H-doRsin⁡ϕcos⁡⁡H].


## 4.2. 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 H and ϕ. That is,
(34)[ΔxReΔyReΔzRe]=[x~Re-xRey~Re-yRez~Re-zRe]=Cte[doR(sin⁡H~-sin⁡H)doR(cos⁡⁡ϕ~cos⁡⁡H~-cos⁡⁡ϕcos⁡⁡H)-doR(sin⁡ϕ~cos⁡⁡H~-sin⁡ϕcos⁡⁡H)].In (34), most variables can be measured by MEMS/GPS and LDS except the ideal attitude angle of ϕ and H. That is to say, angle of ϕ and H 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
(35)ϕ=ϕ~-ΔϕH=H~-ΔH.Involving (35) into (34) and using the same simplification as (28), the transcendental equation can be converted into linear equation:
(36)[ΔxReΔyReΔzRe]=Cte[doRcos⁡⁡H~ΔH-doRsin⁡ϕ~cos⁡⁡H~Δϕ-doRcos⁡⁡ϕ~sin⁡H~ΔH-doRcos⁡⁡ϕ~cos⁡⁡H~Δϕ+doRsin⁡ϕ~sin⁡H~ΔH].Equation (36) can also be described as
(37)[ΔxReΔyReΔzRe]=Cte[0doRcos⁡H~-doRsin⁡ϕ~cos⁡⁡H~-doRcos⁡⁡ϕ~sin⁡H~-doRcos⁡⁡ϕ~cos⁡⁡H~doRsin⁡ϕ~sin⁡H~][ΔϕΔH]=CteC1[ΔϕΔH].Three equations are redundant for the solution of two unknown variables. AssumingC=CteC1 and involving CT, Δϕ and ΔH can be estimated by the least-square method [16]:
(38)[Δϕ^ΔH^]=(CT·C)-1CT[ΔxReΔyReΔzRe].Then,ϕ^=ϕ~-Δϕ^ and H^=H~-Δ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.


## 5. Simulation for the Calibration and the Target Positioning

### 5.1. Simulation for the Calibration

Performance of the calibration is very important for the positioning accuracy of the target. In order to verify the performance of the calibration, simulation results are provided by giving various conditions. During the simulation, all errors of MEMS/GPS and LDS are the same as the former. For the reference point, there is a random error of 0.05 m which is influenced by measurement. Three conditions are given during the simulation.Condition
1. Only the attitude error of MEMS/GPS and random error of R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.Condition
2. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.Condition
3. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=400m.Condition
4. Only the attitude error of MEMS/GPS and random error of R are involved, ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=200m.Condition
5. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=200m.Condition
6. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=400m.As shown in Figure4(a), the calibration algorithm can estimate the angle of ΔH and Δϕ, the residual errors of ΔH 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 ΔH is about 0.07°, and the residual error of Δϕ is about 0.01° in Figure 4(b). But with the increasing of doR, the residual error of ΔH 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 doR. 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.Performance verification for the calibration algorithm.
(a)
Results of condition 1
(b)
Results of condition 2
(c)
Results of condition 3
(d)
Results of condition 4
(e)
Results of condition 5
(f)
Results of condition 6


### 5.2. 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.Condition
1. Only the attitude error of MEMS/GPS and random error of R are involved: ΔH=0.2° and Δϕ=0.1°, doR=400m.Condition
2. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=400m.Condition
3. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.As shown in Figures5(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 ΔH^ are in fact the equivalent estimation of attitude error which will be influenced by the position error of o and ranging error of d. 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 doR=200m. This is perfectly matched with the simulation result of Figure 4.Performance verification for the calibration algorithm.
(a)
Results of condition 1
(b)
Results of condition 2
(c)
Results of conditions 2 and 3


## 5.1. Simulation for the Calibration

Performance of the calibration is very important for the positioning accuracy of the target. In order to verify the performance of the calibration, simulation results are provided by giving various conditions. During the simulation, all errors of MEMS/GPS and LDS are the same as the former. For the reference point, there is a random error of 0.05 m which is influenced by measurement. Three conditions are given during the simulation.Condition
1. Only the attitude error of MEMS/GPS and random error of R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.Condition
2. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.Condition
3. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=400m.Condition
4. Only the attitude error of MEMS/GPS and random error of R are involved, ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=200m.Condition
5. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=200m.Condition
6. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°sin⁡(2πt/50), Δϕ=0.1°sin⁡(2πt/50), and doR=400m.As shown in Figure4(a), the calibration algorithm can estimate the angle of ΔH and Δϕ, the residual errors of ΔH 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 ΔH is about 0.07°, and the residual error of Δϕ is about 0.01° in Figure 4(b). But with the increasing of doR, the residual error of ΔH 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 doR. 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.Performance verification for the calibration algorithm.
(a)
Results of condition 1
(b)
Results of condition 2
(c)
Results of condition 3
(d)
Results of condition 4
(e)
Results of condition 5
(f)
Results of condition 6


## 5.2. 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.Condition
1. Only the attitude error of MEMS/GPS and random error of R are involved: ΔH=0.2° and Δϕ=0.1°, doR=400m.Condition
2. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=400m.Condition
3. All errors of MEMS/GPS, LDS, and R are involved: ΔH=0.2°, Δϕ=0.1°, and doR=200m.As shown in Figures5(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 ΔH^ are in fact the equivalent estimation of attitude error which will be influenced by the position error of o and ranging error of d. 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 doR=200m. This is perfectly matched with the simulation result of Figure 4.Performance verification for the calibration algorithm.
(a)
Results of condition 1
(b)
Results of condition 2
(c)
Results of conditions 2 and 3


## 6. Conclusions

This study has developed a new method for target navigation and mapping. The results indicate that the proposed method can measure position, height, height difference, slant range, and horizontal range for static and moving target. After the calibration of MEMS/GPS, the algorithm can realize precise positioning with the error minor to 2 meters for the static and moving target within 1 kilometer range. The stable and superior performance make the new algorithm very suitable for the target mapping and navigation on land.



---

*Source: 918423-2014-06-29.xml*