# An Accurate and Fault-Tolerant Target Positioning System for Buildings Using Laser Rangefinders and Low-Cost MEMS-Based MARG Sensors

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Literature Review

#### 2.1. Target Tracking/Positioning Based on Sensor Fusion

#### 2.2. MARG-Based AHRS Algorithms

#### 2.3. Laser Aiding Approaches

#### 2.4. Sensor Fusion Algorithms

## 3. Overall Design of the Target Positioning System

## 4. Target Positioning Algorithm and Aiding Systems

#### 4.1. Target Positioning Algorithm

_{e}Y

_{e}Z

_{e}): the frame origin is the Earth center and the axes OX

_{e}, OY

_{e}, and OZ

_{e}are fixed with respect to the Earth. The axis OZ

_{e}lies along the Earth’s polar axis, and the axis OX

_{e}lies along the intersection of the plane of the Greenwich meridian with the Earth’s equatorial plane.

_{n}Y

_{n}Z

_{n}): this frame is a local geographic frame, whose origin O is set at the LR location, and its axes are aligned with the directions of the North, East and the local vertical (up).

_{b}Y

_{b}Z

_{b})): this frame is an orthogonal axis set, whose origin is the LR mass center, and its axes are aligned with the roll, pitch and yaw axes of the LR.

_{b}axis of b-frame. Then, the target position within the n-frame ${p}_{\text{n}}$ can be expressed as:

_{b}axis of b-frame). The rotation around Y

_{b}causes no changes of the target position, so the LR only needs to rotate around X

_{b}and Z

_{b}axes of the b-frame to point toward the target. Second, instead of attitude, the functions of the attitude angles (i.e., $\mathrm{sin}\text{\psi}\mathrm{cos}\text{\theta}$, $\mathrm{cos}\text{\psi}\mathrm{cos}\text{\theta}$ and $\mathrm{sin}\text{\theta}$) are the direct factors influencing the target positioning. Besides, they are the elements in the second column of DCM ${C}_{\text{b}}^{\text{n}}$.

#### 4.2. Aiding Systems for Attitude Determination

#### 4.2.1. Accelerometer/Magnetometer Aiding System

_{x}, A

_{y}and A

_{z}represent the components of gravity along the axes of b-frame. The vector of gravity in b-frame ${g}_{\text{b}}$ can be expressed as:

_{b}axis with a pitch angle θ, which generates the coordinate OX

_{b}Y

_{h}Z

_{1}. The second rotation performs around Y

_{h}with the roll angle r, which generates the coordinate OX

_{h}Y

_{h}Z

_{h}. With this transformation, the h-frame has a horizontal plane OX

_{h}Y

_{h}as shown in Figure 3.

#### 4.2.2. Laser Rangefinder Aiding System

**Figure 5.**Calculation of the attitude changes. (

**a**) Calculation of the yaw angle change; (

**b**) Calculation of the pitch angle change.

## 5. Design of the Federated Kalman Filter

#### 5.1. Local Kalman Filters

#### 5.1.1. Dynamic Models

#### 5.1.2. Observation Models

#### 5.2. FKF Fusion

## 6. Simulation and Experimental Results

#### 6.1. Simulation Results

#### 6.1.1. Performance of the Two LFs and Positioning Accuracy

LF 1 | LF 2 | |
---|---|---|

$\mathrm{sin}\text{\theta}$ | 0.0010 | $7.0733\times {10}^{-4}$ |

$\mathrm{cos}\text{\psi}\mathrm{sin}\text{\theta}$ | 0.0014 | $4.8665\times {10}^{-4}$ |

$\mathrm{cos}\text{\psi}\mathrm{cos}\text{\theta}$ | $9.4631\times {10}^{-4}$ | $7.4538\times {10}^{-4}$ |

Mean (m) | SD (m) | |
---|---|---|

LF 1 | 0.06542 | 0.05014 |

LF 2 | 0.0195 | 0.0001697 |

#### 6.1.2. Fault-Tolerant Capability of FKF

Mean (m) | SD (m) | |
---|---|---|

Fault LF 1 | 18.63 | 3.577 |

LF 2 | 0.09069 | 0.006407 |

FKF | 0.2573 | 0.04489 |

#### 6.2. Experimental Results

#### 6.2.1. Accuracy of the Target Positioning System

Mean (m) | SD (m) | |
---|---|---|

Only Gyros | 15.12 | 0.5471 |

MARG-aided | 0.5698 | 0.01404 |

LR-aided | 0.5726 | $8.098\times {10}^{-11}$ |

MARG and LR | 0.5721 | 0.000169 |

_{b}axis of the b-frame in the system. The errors of the installation angle will break the laws of the target positioning algorithm in Equation (2) and then the positioning accuracy is reduced. Therefore, the installation errors cannot be ignored and some effective calibration approaches like those described in [43] should be added to the proposed system.

#### 6.2.2. System Fault-Tolerant Capability

^{−2}G SD. Figure 14 presents the positioning errors to show the fault tolerance. Table 5 gives the statistical analysis of the positioning errors during the disturbances.

Mean (m) | SD (m) | |
---|---|---|

MARG-aided | 5.609 | 0.1231 |

LR-aided | 0.5726 | $8.098\times {10}^{-11}$ |

MARG and LR | 0.5901 | 0.000271 |

## 7. Conclusions

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## References

- Cheng, J.; Landry, R.; Chen, D.; Guan, D. A novel method for target navigation and mapping based on laser ranging and MEMS/GPS navigation. J. Appl. Math.
**2014**, 2014. [Google Scholar] [CrossRef] - Ma, D.M.; Shiau, J.K.; Wang, I.C.; Lin, Y.H. Attitude determination using a MEMS-based flight information measurement unit. Sensors
**2012**, 12, 1–23. [Google Scholar] [CrossRef] [PubMed] - Li, Y.; Efatmaneshnik, M.; Dempster, A.G. Attitude determination by integration of MEMS inertial sensors and GPS for autonomous agriculture applications. GPS Solut.
**2012**, 16, 41–52. [Google Scholar] [CrossRef] - Li, D.; Landry, R.; Lavoie, P. Low-cost MEMS sensor-based attitude determination system by integration of magnetometers and GPS: A real-data test and performance evaluation. In Proceedings of the 2008 IEEE/ION Position, Location and Navigation Symposium (PLANS), Monterey, CA, USA, 5–8 May 2008; pp. 1190–1198.
- Sabatini, A.M. Estimating three-dimensional orientation of human body parts by Inertial/Magnetic sensing. Sensors
**2011**, 11, 1489–1525. [Google Scholar] [CrossRef] [PubMed] - Li, Y.; Dempster, A.; Li, B.; Wang, J.; Rizos, C. A low-cost attitude heading reference system by combination of GPS and magnetometers and MEMS inertial sensors for mobile applications. Positioning
**2006**, 5, 88–95. [Google Scholar] [CrossRef] - Afzal, M.H.; Renaudin, V.; Lachapelle, G. Magnetic field based heading estimation for pedestrian navigation environments. In Proceedings of the 2011 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Guimaraes, Portugal, 21–23 September 2011; pp. 1–10.
- Bosch. GLM-80 Laser Distance and Angle Measurer. Available online: http://www.bosch-pt.com/productspecials/professional/glm80/cn/en/index.html (accessed on 11 June 2015).
- Yadav, N.; Bleakley, C. Accurate orientation estimation using AHRS under conditions of magnetic distortion. Sensors
**2014**, 14, 20008–20024. [Google Scholar] [CrossRef] [PubMed] - Liu, S.; Atia, M.M.; Karamat, T.; Givigi, S.; Noureldin, A. A dual-rate multi-filter algorithm for LiDAR-aided indoor navigation systems. In Proceeding of the 2014 IEEE/ION Position, Location and Navigation Symposium (PLANS), Monterey, CA, USA, 5–8 May 2014; pp. 1014–1019.
- Carlson, N.A. Federated filter for fault-tolerant integrated navigation systems. In Proceedings of the 1988 IEEE Location and Navigation Symposium (PLANS), Orlando, FL, USA, 29 November–2 December 1988; pp. 110–119.
- Luo, R.C.; Chen, O.; Lin, P.H. Indoor robot/human localization using dynamic triangulation and wireless pyroelectric infrared sensory fusion approaches. In Proceedings of the IEEE International Conference on Robotics and Automation, Saint Paul, MN, USA, 14–18 May 2012; pp. 1359–1364.
- Luo, R.C.; Chen, O. Wireless and pyroelectric sensory fusion system for indoor human/robot localization and monitoring. IEEE-ASME Trans. Mechatron.
**2013**, 18, 845–853. [Google Scholar] [CrossRef] - Tisdale, J.; Ryan, A.; Kim, Z.; Tornqvist, D.; Hedrick, J.K. A multiple UAV system for vision-based search and localization. In Proceedings of the 2008 American Control Conference, Seattle, WA, USA, 11–13 June 2008; pp. 1985–1990.
- Campbell, M.E.; Wheeler, M. A vision based geolocation tracking system for UAV’s. In Proceedings of the AIAA Guidance, Navigation and Control Conference and Exhibit, Keystone, CO, USA, 21–24 August 2006; pp. 1–18.
- Shi, Z.; Sun, Y.; Xiong, L.; Hu, Y.; Yin, B. A multisource heterogeneous data fusion method for pedestrian tracking. Math. Probl. Eng.
**2015**, 2015, 1–10. [Google Scholar] - Jing, L.; Vadakkepat, P. Improved particle filter in sensor fusion for tracking random moving object. In Proceedings of the Instrumentation and Measurement Technology Conference (IMTC 2004), Como, Italy, 18–20 May 2004; pp. 476–481.
- Tzschichholz, T.; Boge, T.; Schilling, K. Relative pose estimation of satellites using PMD-/CCD-sensor data fusion. Acta Astronaut.
**2015**, 109, 25–33. [Google Scholar] [CrossRef] - Aghili, F.; Kuryllo, M.; Okouneva, G.; English, C. Robust vision-based pose estimation of moving objects for automated rendezvous & docking. In Proceedings of the 2010 International Conference on Mechatronics and Automation (ICMA), Xi’an, China, 4–7 August 2010; pp. 305–311.
- Gebre-Egziabher, D.; Hayward, R.C.; Powell, J.D. Design of multi-sensor attitude determination systems. IEEE Trans. Aerosp. Electron. Syst.
**2004**, 40, 627–649. [Google Scholar] [CrossRef] - Emura, S.; Tachi, S. Sensor fusion based measurement of human head motion. In Proceedings of the 3rd IEEE International Workshop on Robot and Human Communication, Nagoya, Japan, 18–20 July 1994; pp. 124–129.
- Han, S.; Wang, J. A novel method to integrate IMU and magnetometers in attitude and heading reference systems. J. Navig.
**2011**, 64, 727–738. [Google Scholar] [CrossRef] - Benson, D.O. A comparison of two approaches to pure-inertial and Doppler-inertial error analysis. IEEE Trans. Aerosp. Electron. Syst.
**1975**, aes-11, 447–455. [Google Scholar] [CrossRef] - Li, W.; Wang, J. Effective adaptive Kalman filter for MEMS-IMU/magnetometers integrated attitude and heading reference systems. J. Navig.
**2013**, 66, 99–113. [Google Scholar] [CrossRef] - Phuong, N.H.Q.; Kang, H.J.; Suh, Y.S.; Ro, Y.S. A DCM based orientation estimation algorithm with an inertial measurement unit and a magnetic compass. J. Univers. Comput. Sci.
**2009**, 15, 859–876. [Google Scholar] - Ercan, Z.; Sezer, V.; Heceoglu, H.; Dikilitas, C.; Gokasan, M.; Mugan, A.; Bogosyan, S. Multi-sensor data fusion of DCM based orientation estimation for land vehicles. In Proceedings of the 2011 IEEE International Conference on Mechatronics (ICM), Istanbul, Turkey, 13–15 April 2011; pp. 672–677.
- Soloviev, A.; Bates, D.; Graas, F. Tight coupling of laser scanner and inertial measurements for a fully autonomous relative navigation solution. Navigation
**2007**, 54, 189–205. [Google Scholar] [CrossRef] - Karras, G.C.; Kyriakopoulos, K.J. Localization of an underwater vehicle using an IMU and a laser-based vision system. In Proceedings of the IEEE Mediterranean Conference on Control & Automation, Athens, Greece, 27–29 June 2007; pp. 1–6.
- Ali, A.; El-Sheimy, N. Low-cost MEMS-based pedestrian navigation technique for GPS-denied areas. J. Sens.
**2013**, 2013, 1–10. [Google Scholar] [CrossRef] - Hellmers, H.; Norrdine, A.; Blankenbach, J.; Eichhorn, A. An IMU/magnetometer-based indoor positioning system using Kalman filtering. In Proceedings of the 2013 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Montbeliard-Belfort, France, 28–31 October 2013; pp. 1–9.
- Bachrach, A.; Prentice, S.; He, R.; Roy, N. Range-robust autonomous navigation in GPS-denied environments. J. Field Robot.
**2011**, 28, 644–666. [Google Scholar] [CrossRef] - Georgy, J.; Noureldin, A.; Korenberg, M.J.; Bayoumi, M.M. Low-cost three-dimensional navigation solution for RISS/GPS integration using mixture particle filter. IEEE Trans. Veh. Technol.
**2010**, 59, 599–615. [Google Scholar] [CrossRef] - Colombo, A.; Fontanelli, D.; Macii, D.; Palopoli, L. Flexible indoor localization and tracking based on a wearable platform and sensor data fusion. IEEE Trans. Instrum. Meas.
**2014**, 63, 864–876. [Google Scholar] [CrossRef] - Carlson, N.A. Federated filter for distributed navigation and tracking applications. In Proceedings of the 58th Annual Meeting of the Institute of Navigation and CIGTF 21st Guidance Test Symposium, Albuquerque, NM, USA, 24–26 June 2002; pp. 340–353.
- Carlson, N.A. Federated square root filter for decentralized parallel processes. IEEE Trans. Aerosp. Electron. Syst.
**1990**, 26, 517–525. [Google Scholar] [CrossRef] - Bancroft, J.B.; Lachapelle, G. Data fusion algorithms for multiple inertial measurement units. Sensors
**2011**, 11, 6771–6798. [Google Scholar] [CrossRef] [PubMed] - Woffinden, D.C.; Geller, D.K. Relative angles-only navigation and pose estimation for autonomous orbital rendezvous. J. Guid. Control Dyn.
**2007**, 30, 1455–1469. [Google Scholar] [CrossRef] - Titterton, D.; Weston, J. Strapdown Inertial Navigation Technology, 2nd ed.; the Institution of Electrical Engineers: Stevenage, UK, 2004; pp. 21–22. [Google Scholar]
- Roetenberg, D.; Luinge, H.J.; Baten, C.T.M.; Veltink, P.H. Compensation of magnetic disturbances improves inertial and magnetic sensing of human body segment orientation. IEEE Trans. Neural Syst. Rehabil. Eng.
**2005**, 13, 395–405. [Google Scholar] [CrossRef] [PubMed] - Koo, W.; Sung, S.; Lee, Y.J. Error calibration of magnetometer using nonlinear integrated filter model with inertial sensors. IEEE Trans. Magn.
**2009**, 45, 2740–2743. [Google Scholar] - Grewal, M.; Andrews, A. Kalman Filtering Theory and Practice Using MATLAB, 3rd ed.; John Wiley & Sons, Inc.: Hoboken, NJ, USA, 2008; pp. 138–140. [Google Scholar]
- ST. STSTEVAL-MKI119V1board. Available online: http://www.st.com/web/en/catalog/tools/PF252689 (accessed on 11 June 2015).
- Cheng, J.; Chen, D.; Sun, X.; Wang, T. A simultaneously calibration approach for installation and attitude errors of an INS/GPS/LDS target tracker. Sensors
**2015**, 15, 3575–3592. [Google Scholar] [CrossRef] [PubMed]

© 2015 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Zhao, L.; Guan, D.; Jr. Landry, R.; Cheng, J.; Sydorenko, K.
An Accurate and Fault-Tolerant Target Positioning System for Buildings Using Laser Rangefinders and Low-Cost MEMS-Based MARG Sensors. *Sensors* **2015**, *15*, 27060-27086.
https://doi.org/10.3390/s151027060

**AMA Style**

Zhao L, Guan D, Jr. Landry R, Cheng J, Sydorenko K.
An Accurate and Fault-Tolerant Target Positioning System for Buildings Using Laser Rangefinders and Low-Cost MEMS-Based MARG Sensors. *Sensors*. 2015; 15(10):27060-27086.
https://doi.org/10.3390/s151027060

**Chicago/Turabian Style**

Zhao, Lin, Dongxue Guan, René Jr. Landry, Jianhua Cheng, and Kostyantyn Sydorenko.
2015. "An Accurate and Fault-Tolerant Target Positioning System for Buildings Using Laser Rangefinders and Low-Cost MEMS-Based MARG Sensors" *Sensors* 15, no. 10: 27060-27086.
https://doi.org/10.3390/s151027060