# Georeferencing of Laser Scanner-Based Kinematic Multi-Sensor Systems in the Context of Iterated Extended Kalman Filters Using Geometrical Constraints

^{*}

## Abstract

**:**

## 1. Introduction

#### 1.1. Georeferencing of Kinematic Multi-Sensor Systems

#### 1.2. Kalman Filter Techniques for Georeferencing

#### 1.3. Contribution

#### 1.4. Outline

## 2. General Georeferencing Approach by Means of Recursive State Estimation

- Which types of sensor observations (e.g., laser scanner, GNSS, IMU, total station) are available and what are their accuracies?
- Which suitable and reliable prior information (e.g., geometrical circumstances, landmarks, maps) are available?
- What is the mathematical relationship between all input data?
- What information about the physical model of the system is known?

#### 2.1. Iterated Extended Kalman Filter with Nonlinear Implicit Measurement Equation

Algorithm 1: Iterated extended Kalman filter (IEKF) with nonlinear implicit measurement equation and nonlinear equality state constraints. |

#### 2.2. Inequality State Constraints by Means of Probability Density Function Truncation

Algorithm 2: Probability density function (PDF) truncation for inequality state constraints. |

## 3. Application in Terms of Accurate Indoor Georeferencing of a k-TLS

#### 3.1. Overview

#### 3.2. Methodology

#### 3.2.1. Observation Vector

#### 3.2.2. Assignment Algorithm for Distinctive Planes

#### 3.2.3. Measurement Equation and State Parameter Vector

#### 3.2.4. System Equation

#### 3.2.5. Nonlinear Equality and Inequality Constraint for the State Parameters

#### 3.2.6. Initialization

#### 3.3. Results

## 4. Discussion

## 5. Conclusions

## Author Contributions

## Funding

## Conflicts of Interest

## References

- Vogel, S.; Alkhatib, H.; Neumann, I. Accurate Indoor Georeferencing with Kinematic Multi Sensor Systems. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation (IPIN), Alcala de Henares (Madrid), Spain, 4–7 October 2016; pp. 1–8. [Google Scholar] [CrossRef]
- Paffenholz, J.A. Direct Geo-Referencing of 3D Point Clouds with 3D Positioning Sensors. Ph.D. Thesis, Deutsche Geodätische Komission (DGK), München, Germany, 2012. [Google Scholar]
- Mautz, R. The challenges of indoor environments and specification on some alternative positioning systems. In Proceedings of the 6th Workshop on Positioning, Navigation and Communication (WPNC), Hannover, Germany, 19 March 2009; pp. 29–36. [Google Scholar] [CrossRef]
- Keller, F.; Sternberg, H. Multi-Sensor Platform for Indoor Mobile Mapping: System Calibration and Using a Total Station for Indoor Applications. Remote Sens.
**2013**, 5, 5805–5824. [Google Scholar] [CrossRef][Green Version] - Jung, J.; Yoon, S.; Ju, S.; Heo, J. Development of Kinematic 3D Laser Scanning System for Indoor Mapping and As-Built BIM Using Constrained SLAM. Sensors
**2015**, 15, 26430–26456. [Google Scholar] [CrossRef] [PubMed][Green Version] - Hartmann, J.; Paffenholz, J.A.; Strübing, T.; Neumann, I. Determination of Position and Orientation of LiDAR Sensors on Multisensor Platforms. J. Surv. Eng.
**2017**, 143, 1–11. [Google Scholar] [CrossRef] - Abmayr, T.; Härtl, F.; Hirzinger, G.; Burschka, D.; Fröhlich, C. A correlation based target finder for terrestrial laser scanning. J. Appl. Geod.
**2008**, 2, 131–137. [Google Scholar] [CrossRef] - Nguyen, V.; Harati, A.; Martinelli, A.; Siegwart, R.; Tomatis, N. Orthogonal SLAM: A Step toward Lightweight Indoor Autonomous Navigation. In Proceedings of the International Conference on Intelligent Robots and Systems, Beijing, China, 9–15 October 2006; pp. 5007–5012. [Google Scholar] [CrossRef]
- Lee, K.W.; Wijesoma, S.; Guzmán, J.I. A constrained SLAM approach to robust and accurate localisation of autonomous ground vehicles. Robot. Auton. Syst.
**2007**, 55, 527–540. [Google Scholar] [CrossRef] - Jutzi, B.; Weinmann, M.; Meidow, J. Improved UAV-Borne 3D Mapping by Fusing Optical and Laserscanner Data. ISPRS Int. Arch. Photogr. Remote Sens. Spat. Inf. Sci.
**2013**, XL-1/W2, 223–228. [Google Scholar] [CrossRef] - Kaul, L.; Zlot, R.; Bosse, M. Continuous-Time Three-Dimensional Mapping for Micro Aerial Vehicles with a Passively Actuated Rotating Laser Scanner. J. Field Robot.
**2016**, 33, 103–132. [Google Scholar] [CrossRef] - Schön, S.; Brenner, C.; Alkhatib, H.; Coenen, M.; Dbouk, H.; Garcia-Fernandez, N.; Fischer, C.; Heipke, C.; Lohmann, K.; Neumann, I.; et al. Integrity and Collaboration in Dynamic Sensor Networks. Sensors
**2018**, 18, 21. [Google Scholar] [CrossRef] [PubMed] - Dang, T. Kontinuierliche Selbstkalibrierung von Stereokameras: Dissertation; Schriftenreih/Institut für Messund Regelungstechnik, KIT, Univ.-Verl. Karlsruhe: Karlsruhe, Germany, 2007; Volume 8. [Google Scholar]
- Steffen, R.; Beder, C. Recursive Estimation with Implicit Constraints. In Pattern recognition; Lecture Notes in Computer Science; Hamprecht, F.A., Schnörr, C., Jähne, B., Eds.; Springer: Berlin, Germany, 2007; Volume 4713, pp. 194–203. [Google Scholar]
- Dang, T. An Iterative Parameter Estimation Method for Observation Models with Nonlinear Constraints. Metrol. Meas. Syst.
**2008**, 15, 421–432. [Google Scholar] - Vogel, S.; Alkhatib, H.; Neumann, I. Iterated Extended Kalman Filter with Implicit Measurement Equation and Nonlinear Constraints for Information-Based Georeferencing. In Proceedings of the 21st International Conference on Information Fusion (FUSION), IEEE, Cambridge, UK, 10–13 July 2018; pp. 1209–1216. [Google Scholar] [CrossRef]
- Ettlinger, A.; Neuner, H.; Burgess, T. Development of a Kalman Filter in the Gauss-Helmert Model for Reliability Analysis in Orientation Determination with Smartphone Sensors. Sensors
**2018**, 18, 414. [Google Scholar] [CrossRef] [PubMed] - Simon, D. Optimal State Estimation; John Wiley & Sons: Hoboken, NJ, USA, 2006. [Google Scholar]
- Simon, D. Kalman filtering with state constraints: A survey of linear and nonlinear algorithms. IET Control Theory Appl.
**2010**, 4, 1303–1318. [Google Scholar] [CrossRef] - Simon, D.; Simon, D.L. Constrained Kalman filtering via density function truncation for turbofan engine health estimation. Int. J. Syst. Sci.
**2010**, 41, 159–171. [Google Scholar] [CrossRef][Green Version] - Hartmann, J.; Trusheim, P.; Alkhatib, H.; Paffenholz, J.A.; Diener, D.; Neumann, I. High Accurate Pointwise (Geo-)Referencing of a k-TLS based Multi-Sensor-System. ISPRS Ann. Photogr. Remote Sens. Spat. Inf. Sci.
**2018**, IV-4, 81–88. [Google Scholar] [CrossRef] - Zoller + Fröhlich GmbH. Z + F Imager 5016, 3D Laser Scanner–Data Sheet. Available online: https://www.zf-laser.com/Z-F-IMAGER-R-5016.184.0.html (accessed on 12 October 2018).
- Hexagone Metrology. Leica Absolute Tracker AT960 (Product Brochure): Absolute Portability. Absolute Speed. Absolute Accuracy. Available online: https://w3.leica-geosystems.com/downloads123/m1/metrology/general/brochures/leica%20at960\%20brochure_en.pdf (accessed on 12 October 2018).
- Roumeliotis, S.I.; Sukhatme, G.S.; Bekey, G.A. Circumventing dynamic modeling: evaluation of the error-state Kalman filter applied to mobile robot localization. In Proceedings of the IEEE International Conference on Robotics and Automation, Piscataway, NJ, USA, 1999; pp. 1656–1663. [Google Scholar] [CrossRef]
- Madyastha, V.; Ravindra, V.; Mallikarjunan, S.; Goyal, A. Extended Kalman Filter vs. Error State Kalman Filter for Aircraft Attitude Estimation. In AIAA Guidance, Navigation, and Control Conference; American Institute of Aeronautics and Astronautics: Reston, VA, USA, 2011. [Google Scholar] [CrossRef]
- Boada, B.L.; Garcia-Pozuelo, D.; Boada, M.J.L.; Diaz, V. A Constrained Dual Kalman Filter Based on pdf Truncation for Estimation of Vehicle Parameters and Road Bank Angle: Analysis and Experimental Validation. IEEE Trans. Intell. Trans. Syst.
**2017**, 18, 1006–1016. [Google Scholar] [CrossRef] - Bar-Shalom, Y.; Li, X.R.; Kirubarajan, T. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software; Wiley: New York, NY, USA, 2001. [Google Scholar]
- Deutsches Institut für Normung e.V. Toleranzen im Hochbau - Bauwerke (DIN 18202), 05.2015. Available online: https://bvn.de/Mitgliederservice/Infoline/Downloads/Technik/Toleranzen_im_Hochbau_2015.pdf (accessed on 12 October 2018).
- Roese-Koerner, L.R. Convex Optimization for Inequality Constrained Adjustment Problems. Ph.D. Thesis, Deutsche Geodätische Komission (DGK), München, Germany, 2015. [Google Scholar]

**Figure 1.**Schematic procedure of the universal recursive filter approach for georeferencing of a kinematic multi-sensor systems (MSS). Steps of the iterated extended Kalman filter (IEKF) (grey) are depicted with possible requested states (yellow), available observations (green) and known prior information (blue). Respective uncertainty information are depicted by red target circles.

**Figure 2.**A general view of the kinematic MSS with its coordinate systems (

**a**) used in the basement of the Geodetic Institute Hannover (GIH) (

**b**).

**Figure 3.**Top view of the measured trajectory obtained by the laser tracker. Two visualizations of the same trajectory in order to highlight the almost linear course. The black curve is regarding the left y-axis (meter) and the red curve regarding the right y-axis (centimetre).

**Figure 4.**Georeferenced 3D point cloud of the environment measured based on the reference pose by means of laser tracker and T-Probe. Original scan points with colors by means of intensity (

**a**). Assigned scan points regarding the left wall (yellow), right wall (blue), ceiling (red) and floor (green) (

**b**).

**Figure 5.**Schematic overview of the 500 replications performed for two types of IMUs as required input data for the iterated extended Kalman filter (IEKF) from Section 2.1 and its related combination $C=I\dots X$ of applied state constraints. The Roman numerals refer to respective state constraints applied regarding Table 3.

**Figure 6.**Mean change in position (top) and orientation (bottom) of the kinematic MSS by means of 500 simulated moderate IMU poses (

**a**) and accurate IMU poses (

**b**) over K epochs. True change in position (top) and orientation (bottom) of the kinematic MSS by means of laser tracker pose (

**c**) over K epochs.

**Figure 7.**Moderate IMU: temporal progress of the median of the root mean square error (RMSE) for position (

**a**) and mean error (ME) for orientation (

**b**) by means of 500 replications for respective combinations of the state constraints applied. The Roman numerals refer to respective state constraints applied regarding Table 3.

**Figure 8.**Accurate IMU: temporal progress of the median of the RMSE for position (

**a**) and ME for orientation (

**b**) by means of 500 replications for respective combinations of the state constraints applied. The Roman numerals refer to respective state constraints applied regarding Table 3.

**Figure 9.**Histograms of the RMSE for position by means of 500 replications for combination III (moderate IMU (

**a**)) and combination III (accurate IMU (

**b**)) of state constraints applied. Related histograms of the ME for orientation by means of 500 replications for combination X (moderate IMU (

**c**)) and combination III (accurate IMU (

**d**)) of state constraints applied. Respective mean is given by a red bar and respective median by a green bar. The Roman numerals refer to respective state constraints applied regarding Table 3.

**Table 1.**Scheduled standard deviations $\sigma $ for the variance-covariance matrix (VCM) ${\mathbf{\Sigma}}_{\mathit{vv}}$ of the observation vector ${\mathit{l}}_{\mathit{k}}$.

Sensor Type | Observation | Assumed $\mathit{\sigma}$ | |
---|---|---|---|

Moderate IMU | Accurate IMU | ||

Laser scanner | $x,y,z$ | 3 mm | 3 mm |

IMU | X | 0.01 mm | 0.01 mm |

$Y,Z$ | 80 mm | 20 mm | |

$\Omega ,\Phi ,K$ | 0.2${}^{\circ}$ | 0.07${}^{\circ}$ |

**Table 2.**Scheduled standard deviations $\sigma $ for the initial VCM ${\mathbf{\Sigma}}_{\mathit{xx},0}$ of the initial state vector ${\mathit{x}}_{0}$.

State Parameter | $\mathit{\sigma}$ |
---|---|

$\Delta X,\Delta Y,\Delta Z$ | 0.1 m |

$\Delta \Omega ,\Delta \Phi ,\Delta \mathit{K}$ | ${5.7}^{\circ}$ |

$\Delta {v}_{x},\Delta {v}_{y},\Delta {v}_{z}$ | 0.1 m/s |

${\mathit{n}}_{{\mathit{e}}_{\mathit{x}}},{\mathit{n}}_{{\mathit{e}}_{\mathit{y}}},{\mathit{n}}_{{\mathit{e}}_{\mathit{z}}}$ | $0.1$ |

${\mathit{d}}_{\mathit{e}}$ | 0.1 m |

**Table 3.**Investigated combinations of respective equality (red) and inequality (green) state constraints. Applied constraints within each combination are depicted with a $\u2713$ symbol.

Combinations of Respective Equality and Inequality State Constraints | ||||||||||
---|---|---|---|---|---|---|---|---|---|---|

I | II | III | IV | V | VI | VII | VIII | IX | X | |

unit vector for left wall | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | |

unit vector for right wall | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | |

unit vector for ceiling | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | |

unit vector for floor | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | ✓ | |

left/right wall are parallel | ✓ | ✓ | ✓ | ✓ | ||||||

ceiling/floor are parallel | ✓ | ✓ | ✓ | ✓ | ||||||

left wall/ceiling are perpendicular | ✓ | ✓ | ✓ | ✓ | ||||||

right wall/floor are perpendicular | ✓ | ✓ |

**Table 4.**Root mean square error (RMSE) for position by means of 500 replications. The Roman numerals refer to respective state constraints applied regarding Table 3. Each of the seven characteristic values (minimum, maximum, mean, median, standard deviation (SD) as well as lower bound (↓) and upper bound (↑) of the $95\%$ confidence interval (CI)) are divided into two additional rows regarding moderate (above) and accurate (below) inertial measurement unit (IMU). The largest (red) and lowest (green) estimates are marked for first five rows.

Combinations of Respective Equality and Inequality State Constraints | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|

IMU | I | II | III | IV | V | VI | VII | VIII | IX | X | |

Min [m] | 12.646 1.9833 | 1.4684 0.8992 | 0.0118 0.0114 | 0.0128 0.0140 | 0.0147 0.0168 | 0.0130 0.0142 | 0.0135 0.0143 | 0.0161 0.0209 | 0.0130 0.0142 | 0.0168 0.0159 | 0.0164 0.0209 |

Max [m] | 13.030 2.0864 | 3469.2 2.4$\xb7{10}^{5}$ | 5.7065 4.3490 | 0.1649 0.1049 | 0.2269 0.1425 | 0.1360 0.0561 | 0.2460 0.1122 | 0.1234 0.0966 | 0.2278 0.0781 | 0.4138 0.2269 | 141.08 0.0699 |

Mean [m] | 12.835 2.0336 | 79.778 1974.7 | 0.3188 0.1828 | 0.0201 0.0174 | 0.0280 0.0312 | 0.0218 0.0182 | 0.0470 0.0327 | 0.0269 0.0304 | 0.0226 0.0206 | 0.0335 0.0282 | 0.3139 0.0291 |

Median [m] | 12.832 2.0337 | 9.9179 8.8968 | 0.1118 0.0607 | 0.0149 0.0145 | 0.0214 0.0273 | 0.0162 0.0157 | 0.0455 0.0284 | 0.0236 0.0286 | 0.0172 0.0180 | 0.0232 0.0244 | 0.0263 0.0275 |

SD [m] | 0.0678 0.0176 | 320.29 16083 | 0.6136 0.3384 | 0.0160 0.0080 | 0.0192 0.0130 | 0.0151 0.0064 | 0.0272 0.0167 | 0.0116 0.0081 | 0.0163 0.0075 | 0.0325 0.0147 | 6.3077 0.0063 |

$\downarrow 95\%$ CI [m] | 12.714 1.9983 | 2.7926 2.0866 | 0.0189 0.0137 | 0.0134 0.0141 | 0.0163 0.0194 | 0.0133 0.0143 | 0.0140 0.0146 | 0.0171 0.0236 | 0.0135 0.0144 | 0.0179 0.0181 | 0.0178 0.0227 |

$\uparrow 95\%$ CI [m] | 12.960 2.0707 | 559.22 7126.2 | 1.9255 0.9893 | 0.0596 0.0405 | 0.0742 0.0647 | 0.0729 0.0409 | 0.1042 0.0771 | 0.0563 0.0525 | 0.0654 0.0414 | 0.1155 0.0570 | 0.0947 0.0498 |

**Table 5.**Mean error (ME) for orientation by means of 500 replications. The Roman numerals refer to respective state constraints applied regarding Table 3. Each of the seven characteristic values (minimum, maximum, mean, median, SD as well as lower bound (↓) and upper bound (↑) of the $95\%$ CI) are divided into two additional rows regarding moderate (above) and accurate (below) IMU. The largest (red) and lowest (green) estimates are marked for first five rows.

Combinations of Respective Equality and Inequality State Constraints | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|

IMU | I | II | III | IV | V | VI | VII | VIII | IX | X | |

Min [°] | 3.6742 0.1548 | 4.6565 2.1602 | 2.9283 0.1522 | 2.9843 0.1541 | 2.9558 0.1986 | 3.0133 0.1557 | 2.9283 0.1498 | 2.9544 0.2151 | 2.9793 0.1535 | 2.9560 0.2084 | 2.9503 0.2100 |

Max [°] | 4.8145 0.4367 | 42.297 32.819 | 4.3253 0.4542 | 4.1203 0.4114 | 4.0770 0.4455 | 4.0935 0.4197 | 4.0896 0.4218 | 4.0788 0.4434 | 4.0887 0.4279 | 4.0779 0.4460 | 4.0766 0.4369 |

Mean [°] | 4.2414 0.2654 | 11.222 9.4379 | 3.6246 0.2727 | 3.6140 0.2371 | 3.5657 0.2864 | 3.5990 0.2447 | 3.5691 0.2628 | 3.5653 0.2860 | 3.5873 0.2562 | 3.5643 0.2857 | 3.5625 0.2856 |

Median [°] | 4.2471 0.2611 | 10.355 8.8777 | 3.6242 0.2649 | 3.6117 0.2322 | 3.5619 0.2821 | 3.5992 0.2397 | 3.5689 0.2600 | 3.5619 0.2822 | 3.5874 0.2520 | 3.5615 0.2825 | 3.5568 0.2809 |

SD [°] | 0.1890 0.0529 | 4.2427 4.1022 | 0.2302 0.0598 | 0.1807 0.0463 | 0.1863 0.0400 | 0.1814 0.0463 | 0.1893 0.0482 | 0.1871 0.0405 | 0.1838 0.0453 | 0.1866 0.0400 | 0.1874 0.0400 |

$\downarrow 95\%$ CI [m] | 3.8684 0.1793 | 5.7887 3.9401 | 3.1598 0.1762 | 3.2611 0.1667 | 3.1979 0.2212 | 3.2463 0.1668 | 3.1921 0.1706 | 3.1992 0.2216 | 3.2241 0.1836 | 3.1982 0.2226 | 3.1970 0.2206 |

$\uparrow 95\%$ CI [m] | 4.6200 0.3758 | 22.2408 19.2219 | 4.0898 0.4079 | 3.9940 0.3426 | 3.9578 0.3809 | 3.9841 0.3443 | 3.9604 0.3699 | 3.9565 0.3787 | 3.9702 0.3612 | 3.9562 0.3822 | 3.9542 0.3804 |

© 2019 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 (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Vogel, S.; Alkhatib, H.; Bureick, J.; Moftizadeh, R.; Neumann, I. Georeferencing of Laser Scanner-Based Kinematic Multi-Sensor Systems in the Context of Iterated Extended Kalman Filters Using Geometrical Constraints. *Sensors* **2019**, *19*, 2280.
https://doi.org/10.3390/s19102280

**AMA Style**

Vogel S, Alkhatib H, Bureick J, Moftizadeh R, Neumann I. Georeferencing of Laser Scanner-Based Kinematic Multi-Sensor Systems in the Context of Iterated Extended Kalman Filters Using Geometrical Constraints. *Sensors*. 2019; 19(10):2280.
https://doi.org/10.3390/s19102280

**Chicago/Turabian Style**

Vogel, Sören, Hamza Alkhatib, Johannes Bureick, Rozhin Moftizadeh, and Ingo Neumann. 2019. "Georeferencing of Laser Scanner-Based Kinematic Multi-Sensor Systems in the Context of Iterated Extended Kalman Filters Using Geometrical Constraints" *Sensors* 19, no. 10: 2280.
https://doi.org/10.3390/s19102280