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

Abstract

Georeferencing is an indispensable necessity regarding operating with kinematic multi-sensor systems (MSS) in various indoor and outdoor areas. Information from object space combined with various types of prior information (e.g., geometrical constraints) are beneficial especially in challenging environments where common solutions for pose estimation (e.g., global navigation satellite system or external tracking by a total station) are inapplicable, unreliable or inaccurate. Consequently, an iterated extended Kalman filter is used and a general georeferencing approach by means of recursive state estimation is introduced. This approach is open to several types of observation inputs and can deal with (non)linear systems and measurement models. The capability of using both explicit and implicit formulations of the relation between states and observations, and the consideration of (non)linear equality and inequality state constraints is a special feature. The framework presented is evaluated by an indoor kinematic MSS based on a terrestrial laser scanner. The focus here is on the impact of several different combinations of applied state constraints and the dependencies of two classes of inertial measurement units (IMU). The results presented are based on real measurement data combined with simulated IMU measurements.

1. Introduction

Multi-sensor systems (MSS) are greatly used nowadays in geodesy to capture an environment for various applications. Georeferencing is required in most cases for these data to be applicable. In simple and straightforward words, georeferencing is to derive the position and orientation of a platform with respect to a superordinate coordinate system. Therefore, in a static case, georeferencing would mean to derive six pose parameters (three translations and three rotations), whereas for kinematic platforms, the six degrees of freedom (DOF) should be calculated separately for each time epoch [1]. In general, there is no need to consider a scale factor as additional DOF, as long as sensors (e.g., laser scanner) of the MSS are consistent with each other during data acquisition. If there is a necessity, extension to a three-dimensional (3D) similarity transformation can be applied [2].

This indispensable necessity of precise and accurate pose parameters is a frequent challenge for outdoor and indoor mapping applications. Depending on respective complex or challenging environments common methods for georeferencing might fail, are unreliable or are at least inaccurate. The main reasons for this are missing or inaccurate observations from a global navigation satellite system (GNSS) within indoor spaces or inner-city environments caused by shadowing or multipath effects. In addition, further methods (e.g., visual odometry) and sensors (e.g., inertial measurement unit (IMU)) have to deal with significant drifts in pose estimation [1,3,4,5]. In order to improve georeferencing for such challenging circumstances, a Kalman filter-based approach is extended and validated by means of a laser scanner-based kinematic MSS within this paper. As a novelty, arbitrary explicit and implicit measurement equations as well as nonlinear equality and inequality state constraints can be applied.

1.1. Georeferencing of Kinematic Multi-Sensor Systems

Georeferencing is generally realized through three different approaches, each of them deals with various methods that are based on the sensors available and environmental conditions. These approaches are called direct, indirect and data-driven georeferencing [2]. In direct georeferencing, position and orientation of a measuring platform are derived directly from the sensors available on board, such as a GNSS antenna [2], an IMU or an external sensor, such as a laser tracker [6] or a total station [4]. However, this approach depends highly on the environmental circumstances (e.g., visibility in complex indoor interior or absence of GNSS observations). In indirect georeferencing, observations of other sensors available on the platform, such as laser scanners or cameras are taken into account. In this approach, common environmental information (e.g., known control points for laser scanners by means of artificial targets) which are captured both in the local sensors’ coordinate system and in a superordinate coordinate system are linked together [7]. Approaches for data-driven georeferencing require point cloud information which has already been georeferenced. This can be given by means of 3D city models, floor plans or other maps of the environment requested. Several arbitrary matching algorithms can be applied to get the position and orientation of an MSS which is acquiring point cloud data regarding models or maps mentioned. Uncertainty of the prior information affects the final georeferencing solution significantly. Known approaches for this method generally rely on iterative closest point (ICP) algorithms or rather on simultaneous localization and mapping (SLAM) methods [5,8,9,10,11].

1.2. Kalman Filter Techniques for Georeferencing

Combinations of aforementioned approaches are also possible and advisable to increase the accuracy and precision of the georeferencing of a kinematic MSS. This data fusion is commonly covered within the system state of a filtering approach. Such recursive approaches enable possibilities to handle big data, which come along with present and future multi-sensor technologies. Furthermore, they are suitable for online applications and usually require less memory and computational effort than batch algorithms that have been adapted for online georeferencing applications [12].

A Kalman filter (KF) is a well-known two-step procedure for this in which the next system state is estimated based on the previous state information and recent observations subsequently. Therefore, an iterative process is required by utilizing nonlinear measurement equations, which seem to be the most logical choice in case of trajectories. Consequently, such a procedure is called iterated extended Kalman filter (IEKF). A standard extended Kalman filter (EKF) can also handle nonlinear equations. However, an IEKF with further iterations is more suitable in the case of high nonlinear functions and will provide more accurate results using only small additional computational effort. Handling with nonlinear equations can also be done by means of unscented transformation (UT) as part of the unscented Kalman filter (UKF).

So far, in almost every research only explicit measurement equations are considered for such filter approaches. This means that the observations are taken into account as a function of the state parameters. Such a model is generally referred to a Gauss–Markov model (GMM). However, the use of a Gauss–Helmert model (GHM), which gives the possibility to implicitly link the observations to the state parameters, has also been studied by a few researchers [13,14,15,16,17]. Such a methodology provides the opportunity to include all kinds of measurement equations into the filtering approach, regardless whether they are of an implicit or explicit nature. A basic algorithm for nonlinear implicit measurement equations within an IEKF for extrinsic auto-calibration of a stereo rig is proposed in [13,15]. The algorithm is used for the extrinsic auto-calibration of a stereo rig which has led to satisfying results. Furthermore, a linear KF with respect to GHM is developed in [17] and applied for orientation determination with smartphone sensors. However, both contributions do not consider state constraints. In addition, the latter is based on a linear KF approach. In [14], implicit measurement equations within a recursive estimation approach for Kalman filtering are referred to as implicit constraints. Usage of implicit measurement equations in terms of an UKF does not exist at all.

1.3. Contribution

Except for [16], the approaches mentioned within Section 1.2 have neglecting additional state constraints in common. Although, it is very useful to consider suitable environmental scene information by means of equality or inequality state constraints. This possibility is frequently used and evidenced in terms of well-known filter approaches with explicit formulations [2,18,19,20]. Horizontal and vertical lines, parallel or perpendicular lines and different planes in a scene are examples of such information which could be used as assigned geometric constraints during data analysis. In the recent work by [16], an IEKF by means of implicit measurement equations and nonlinear equality constraints is used for georeferencing of a simulated kinematic MSS. As a novelty, this approach is extended by nonlinear inequality state constraints within this paper. This increases the possibilities to apply any suitable geometrical prior information and to improve the georeferencing solution even in such challenging environments mentioned. Fundamental applicability is shown by means of a real kinematic MSS within an indoor environment and validated by highly accurate reference information. Additionally, a more general overview of the filter approach is given within this paper to make the approach independent from specific MSS, environments and prior information used.

1.4. Outline

The dedicated sections of this paper are as follows. An overview of the general georeferencing approach by means of a recursive state estimation is introduced in Section 2. This algorithm proposed is confirmed by being applied to a real data-set of an indoor environment that is captured by a kinematic MSS equipped with a TLS and tracked by a laser tracker in Section 3. The paper ends with a discussion of the results presented in Section 4 whereas Section 5 concludes this contribution.

2. General Georeferencing Approach by Means of Recursive State Estimation

A standardized estimation approach is indispensable to ensure an accurate, precise, reliable and complete georeferencing solution of different arbitrary kinematic MSS. The drawbacks mentioned in Section 1 could be eliminated only by providing a generally valid framework, which is applicable to as many use cases and systems as possible. For this reason, a recursive state estimation approach, which is compatible with various types of input data (e.g., requested states, available sensor observations and additional prior information), is formulated in this paper. However, the basic structure and equations used are with respect to [16]. The carefully selected information depends on each individual application and its respective circumstances. However, they are combined and fused in a unified way within the general valid framework to deliver optimal results. Necessary demands on the input data can be divided into four interconnected questions:

• 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?

In theory, all possible input data should be considered. These input possibilities are restricted to the most common ones for the sake of simplification and according to the current paper perspectives. However, it should be noted that other types of input data are also possible and should be considered based on the application. A schematic overview of the universal recursive filter approach for georeferencing of a kinematic MSS together with corresponding relations between states, observations, prior information and respective parts of an IEKF is illustrated in Figure 1. In addition to the input data (states and observations) and prior information, sets of fundamental functions have to be formulated and integrated into the process. An arbitrary system model will describe the physical behaviour of the MSS between neighbouring epochs. Any model from the current state of the art can be selected for this. Total neglection of the system model is also possible and will result in a sequential adjustment approach. Formulation of a measurement model can happen in an implicit and/or explicit manner. (Non)linear functions regarding the states can also be added by means of equality and/or inequality formulations. Such state constraints can be integrated by means of several different methods (e.g., pseudo observations, projection method, probability density function (PDF) truncation or soft constraints). However, equality and inequality constraints have the crucial advantage of including specific further information into the filter approach by means of clear values (in the case of equalities) or thresholds (in the case of inequalities).

2.1. Iterated Extended Kalman Filter with Nonlinear Implicit Measurement Equation

The basic structure of the georeferencing approach applied is based on the IEKF, which was published by [13] and enables the possibility of integrating implicit measurement equations (of type $\mathit{h}(\mathit{l},\mathit{x})=\mathbf{0}$) within the recursive estimation process. This gives the possibility to consider inextricable relationships between states $\mathit{x}$ and observations $\mathit{l}$ within the observation model. Only explicit relationships (of type $\mathit{l}=\mathit{h}\left(\mathit{x}\right)$) are allowed within normal KF, which results in major restrictions. As shown in Section 3, there is an important demand for implicit equations within the IEKF process in order to consider more challenging relationships. However, explicit measurement equations are still possible to use but will be converted into $\mathit{l}-\mathit{h}\left(\mathit{x}\right)=\mathbf{0}$ to fulfil the implicit statement.

There are nonlinear measurement equations $\mathit{h}(·)$ within the IEKF which provide a connection between the observations measured and states requested. The physical behaviour of the MSS over time is formulated within the nonlinear system model $\mathit{f}(·)$ over all epochs $k=1,\dots ,\mathrm{K}$ theoretically:

$$\begin{array}{c}\hfill \mathit{h}\left({\mathit{l}}_{\mathit{k}}+{\mathit{v}}_{\mathit{k}},{\mathit{x}}_{\mathit{k}}\right)=\mathbf{0},{\mathit{v}}_{\mathit{k}}\sim N\left(\mathbf{0},{\mathbf{\Sigma }}_{\mathit{vv}}\right)\end{array}$$

(1)

$$\begin{array}{c}\hfill {\mathit{x}}_{\mathit{k}}=\mathit{f}\left({\mathit{x}}_{\mathit{k}-1},{\mathit{u}}_{\mathit{k}-1},{\mathit{w}}_{\mathit{k}-1}\right),{\mathit{w}}_{\mathit{k}-1}\sim N\left(\mathbf{0},{\mathbf{\Sigma }}_{\mathit{ww}}\right).\end{array}$$

(2)

Here, $\mathit{u}$ is the deterministic control by means of external controls. The variance-covariance matrix (VCM) ${\mathbf{\Sigma }}_{\mathit{ww}}$ of the system noise $\mathit{w}$ and ${\mathbf{\Sigma }}_{\mathit{vv}}$ of the measurement noise $\mathit{v}$ are normally distributed with zero mean. Regarding our universal recursive filter approach, the IEKF is divided into a prediction step using the system model, an update step making use of the measurement model and into a constrained step for applying known prior information by means of linear $\mathit{D}{\mathit{x}}_{\mathit{k}}$ and/or nonlinear $\mathit{g}\left({\mathit{x}}_{\mathit{k}}\right)$ state constraints. All three steps are described within [16] in detail and are summarized in Algorithm 1.

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

It is worth mentioning that up to now, to the best of our knowledge, no research which deals with state constraints in connection with implicit measurement equations are being investigated. All published methods are regarding explicit measurement equations where it is possible to separate states and observations from each other. However, in terms of implicit measurement equations of type $\mathit{h}(\mathit{l},\mathit{x})=\mathbf{0}$, modified state parameters during the constraint step will violate this equation because of the unaffected observations during the constraint step. This fact is currently being dealt with by the authors and is under investigation. However, our results in Section 3 show the fundamental validity.

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

The constrained step within this paper is extended to allow the possibility of considering inequality state constraints. Instead of using the projection method (cf. Algorithm 1, line 28–30), the flexible PDF truncation method is used, and both given in [18] and [20]. In theory, other methods also mentioned above (e.g., pseudo observations) can be applied in order to consider state constraints. However, usage of state constraints in combination with implicit measurement equations is so far not considered for any Kalman filtering technique. By using the PDF truncation, equality and inequality constraints can be included simultaneously by the same method and there is no need to perform inefficient quadratic programming techniques. Furthermore, numerical instabilities resulting from e.g., singular measurement noise covariance in the context of perfect measurements can be avoided [19]. Depending on the respective conditions, the thresholds could be set by means of lower ${\mathit{lb}}_{\mathit{i},\mathit{k}}$ and upper ${\mathit{ub}}_{\mathit{i},\mathit{k}}$ boundaries for $\mathit{s}$ scalar two-sided state constraints for any arbitrary nonlinear functions ${\mathit{g}}_{\mathit{i}}$ of the states.

$$\begin{array}{c}\hfill {\mathit{lb}}_{\mathit{i},\mathit{k}}\le {\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\le {\mathit{ub}}_{\mathit{i},\mathit{k}}\mathit{i}=1,\dots ,\mathit{s}.\end{array}$$

(3)

Within this PDF truncation method, the estimated PDF of the IEKF (assumed in this paper as Gaussian) is truncated by means of the defined lower and upper boundaries and, subsequently, recomputed to the constrained estimate at the mean of the truncated PDF. Realization of the PDF truncation method is carried out for every single constraint $\mathit{i}=1\dots \mathit{s}$ successively. Furthermore, ${\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}$ will be the state estimate after applying the $\mathit{i}$-th constraint and ${\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}$ will be its respective VCM. Their initialization for $\mathit{i}=0$ is achieved by updated KF estimations ${\tilde{\mathit{x}}}_{0,\mathit{k}}={\widehat{\mathit{x}}}_{\mathit{k}}^{+}$ and ${\tilde{\mathbf{\Sigma }}}_{0,\mathit{k}}={\mathbf{\Sigma }}_{\widehat{\mathit{x}}\widehat{\mathit{x}},\mathit{k}}^{+}$. Afterwards, a transformation from ${\mathit{x}}_{\mathit{i},\mathit{k}}$ to ${\mathit{z}}_{\mathit{i},\mathit{k}}$ is performed for the decoupling of the $\mathit{s}$ constraints:

$$\begin{array}{c}\hfill {\mathit{z}}_{\mathit{i},\mathit{k}}={\mathit{S}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}^{-\frac{1}{2}}·{\mathit{T}}_{\mathit{i}}^T\left({\mathit{x}}_{\mathit{k}}-{\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}\right).\end{array}$$

(4)

The diagonal matrices ${\mathit{W}}_{\mathit{i}}$ and orthogonal matrices ${\mathit{T}}_{\mathit{i}}$ are obtained by performing Jordan canonical decomposition of the VCM ${\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}$:

$$\begin{array}{c}\hfill {\mathit{T}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}·{\mathit{T}}_{\mathit{i}}^T={\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}.\end{array}$$

(5)

The orthogonal matrix ${\mathit{S}}_{\mathit{i}}$ is determined by using Gram–Schmidt orthogonalization (cf. [18]) and satisfies:

$$\begin{array}{c}\hfill {\mathit{S}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}^{-\frac{1}{2}}·{\mathit{T}}_{\mathit{i}}^T·{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)={\left[\begin{array}{cccc}{\left({\mathit{g}}_{\mathit{i}}{\left({\mathit{x}}_{\mathit{k}}\right)}^T·{\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}·{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\right)}^{\frac{1}{2}}& 0& \dots & 0\end{array}\right]}^T.\end{array}$$

(6)

The normalized scalar constraint could be derived using this transformation, where ${\mathit{z}}_{\mathit{i},\mathit{k}}$ has a zero mean and a VCM of identity.

$$\begin{array}{c}\hfill {\mathit{a}}_{\mathit{i},\mathit{k}}\le \left[\begin{array}{cccc}1& 0& \dots & 0\end{array}\right]·{\mathit{z}}_{\mathit{i},\mathit{k}}\le {\mathit{b}}_{\mathit{i},\mathit{k}}.\end{array}$$

(7)

The transformed boundaries ${\mathit{a}}_{\mathit{i},\mathit{k}}$ and ${\mathit{b}}_{\mathit{i},\mathit{k}}$ are:

$$\begin{array}{c}\hfill {\mathit{a}}_{\mathit{i},\mathit{k}}=\frac{{\mathit{lb}}_{\mathit{i},\mathit{k}}-{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)·{\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}}{{\left({\mathit{g}}_{\mathit{i}}{\left({\mathit{x}}_{\mathit{k}}\right)}^T·{\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}·{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\right)}^{\frac{1}{2}}},{\mathit{b}}_{\mathit{i},\mathit{k}}=\frac{{\mathit{ub}}_{\mathit{i},\mathit{k}}-{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)·{\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}}{{\left({\mathit{g}}_{\mathit{i}}{\left({\mathit{x}}_{\mathit{k}}\right)}^T·{\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}·{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\right)}^{\frac{1}{2}}}.\end{array}$$

(8)

Truncation of the Gaussian PDF by means of the lower an upper bound and the integration variables $\zeta$ and $\gamma$ is implemented by:

$$\begin{array}{c}\hfill {\int }_{{\mathit{a}}_{\mathit{i},\mathit{k}}}^{{\mathit{b}}_{\mathit{i},\mathit{k}}}\frac{1}{\sqrt{2\pi }}·e^{-\frac{{\zeta }^2}{2}}\mathit{d}\zeta =\frac{1}{2}\left[\mathrm{erf}\left(\frac{{\mathit{b}}_{\mathit{i},\mathit{k}}}{\sqrt{2}}\right)-\mathrm{erf}\left(\frac{{\mathit{a}}_{\mathit{i},\mathit{k}}}{\sqrt{2}}\right)\right],\mathrm{erf}\left(u\right)=\frac{2}{\sqrt{\pi }}{\int }_0^u{e}^{-{\gamma }^2}\mathit{d}\gamma .\end{array}$$

(9)

The normalized truncated PDF within the boundaries ${\mathit{a}}_{\mathit{i},\mathit{k}}$ and ${\mathit{b}}_{\mathit{i},\mathit{k}}$ is given by:

$$\begin{array}{c}\hfill \mathrm{pdf}\left(\zeta \right)={\beta }_{i,k}·e^{\frac{-{\zeta }^2}{2}},{\beta }_{i,k}=\frac{\sqrt{2}}{\sqrt{\pi }·\left[\mathrm{erf}\left(\frac{{\mathit{b}}_{\mathit{i},\mathit{k}}}{\sqrt{2}}\right)-\mathrm{erf}\left(\frac{{\mathit{a}}_{\mathit{i},\mathit{k}}}{\sqrt{2}}\right)\right]}.\end{array}$$

(10)

The mean ${\mu }_{i,k}$ and variance ${\sigma }_{i,k}^2$ of the $\mathit{i}$-th element of ${\mathit{z}}_{\mathit{i},\mathit{k}}$ is computed by:

$$\begin{array}{cc}\hfill {\mu }_{i,k}& ={\beta }_{i,k}·\left[e^{\frac{-{\mathit{a}}_{\mathit{i},\mathit{k}}^2}{2}}-e^{\frac{-{\mathit{b}}_{\mathit{i},\mathit{k}}^2}{2}}\right]\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {\sigma }_{i,k}^2& ={\beta }_{i,k}·\left[e^{\frac{-{\mathit{a}}_{\mathit{i},\mathit{k}}^2}{2}}·\left({\mathit{a}}_{\mathit{i},\mathit{k}}-2·{\mu }_{i,k}\right)-e^{\frac{-{\mathit{b}}_{\mathit{i},\mathit{k}}^2}{2}}·\left({\mathit{b}}_{\mathit{i},\mathit{k}}-2·{\mu }_{i,k}\right)\right]+{\mu }_{i,k}^2+1.\hfill \end{array}$$

(12)

With this, the mean ${\tilde{\mathit{z}}}_{\mathit{i}+1,\mathit{k}}$ and VCM ${\tilde{\mathit{C}}}_{\mathit{i}+1,\mathit{k}}$ of the transformed state could be estimated:

$$\begin{array}{cc}\hfill {\tilde{\mathit{z}}}_{\mathit{i}+1,\mathit{k}}& ={\left[\begin{array}{cccc}{\mu }_{i,k}& 0& \dots & 0\end{array}\right]}^T\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {\tilde{\mathit{C}}}_{\mathit{i}+1,\mathit{k}}& =\mathrm{diag}\left({\sigma }_{i,k}^2,1,\dots ,1\right).\hfill \end{array}$$

(14)

The ${\tilde{\mathit{x}}}_{\mathit{i}+1,\mathit{k}}$ and its corresponding VCM ${\tilde{\mathbf{\Sigma }}}_{\mathit{i}+1,\mathit{k}}$ of the state are estimated by means of inversion of transformation in (4):

$$\begin{array}{cc}\hfill {\tilde{\mathit{x}}}_{\mathit{i}+1,\mathit{k}}& ={\mathit{T}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}^{-\frac{1}{2}}·{\mathit{S}}_{\mathit{i}}^T·{\tilde{\mathit{z}}}_{\mathit{i}+1,\mathit{k}}+{\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{\Sigma }}}_{\mathit{i}+1,\mathit{k}}& ={\mathit{T}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}^{-\frac{1}{2}}·{\mathit{S}}_{\mathit{i}}^T·{\tilde{\mathit{C}}}_{\mathit{i}+1,\mathit{k}}·{\mathit{S}}_{\mathit{i}}·{\mathit{W}}_{\mathit{i}}^{-\frac{1}{2}}·{\mathit{T}}_{\mathit{i}}^T.\hfill \end{array}$$

(16)

Finally, after performing this for all s constraints in series, the constrained states ${\tilde{\mathit{x}}}_{\mathit{k}}$ and their VCM ${\tilde{\mathbf{\Sigma }}}_{\mathit{k}}$ could be derived as:

$$\begin{array}{cc}\hfill {\tilde{\mathit{x}}}_{\mathit{k}}& ={\tilde{\mathit{x}}}_{\mathit{s},\mathit{k}}\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill {\tilde{\mathbf{\Sigma }}}_{\mathit{k}}& ={\tilde{\mathbf{\Sigma }}}_{\mathit{s},\mathit{k}}.\hfill \end{array}$$

(18)

The whole procedure of PDF truncation for involving inequality constraints is depicted in Algorithm 2. In order to handle one-sided inequality constraints, ${\mathit{lb}}_{\mathit{i},\mathit{k}}=-\infty$ or ${\mathit{ub}}_{\mathit{i},\mathit{k}}=\infty$ could be used. In the case of equality constraints, Equations (7), (8), (11), (12) are required to be changed to perform PDF truncation:

$$\begin{array}{cc}\hfill {\mathit{c}}_{\mathit{i},\mathit{k}}& =\left[\begin{array}{cccc}1& 0& \dots & 0\end{array}\right]·{\mathit{z}}_{\mathit{i},\mathit{k}}\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill {\mathit{c}}_{\mathit{i},\mathit{k}}& =\frac{{\mathit{d}}_{\mathit{i},\mathit{k}}-{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)·{\tilde{\mathit{x}}}_{\mathit{i},\mathit{k}}}{{\left({\mathit{g}}_{\mathit{i}}{\left({\mathit{x}}_{\mathit{k}}\right)}^T·{\tilde{\mathbf{\Sigma }}}_{\mathit{i},\mathit{k}}·{\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\right)}^{\frac{1}{2}}}\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill {\mu }_{i,k}& ={\mathit{c}}_{\mathit{i},\mathit{k}}\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill {\sigma }_{i,k}^2& =0.\hfill \end{array}$$

(22)

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

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

Various MSS in terms of calibration, acquisition and georeferencing are developed and used in practice at the Geodetic Institute Hannover (GIH) of the Leibniz University Hannover. Within this case study, the proposed general georeferencing approach from Section 2 is applied to a kinematic MSS extensively described in [6,21]. Utilizing such a proved MSS allows us to focus on the application of the proposed theoretical approach and to rely on an already calibrated and synchronized system. Furthermore, highly accurate validation by means of a laser tracker is possible. Such a comparison based on the trajectory is much more accurate than based on the 3D point cloud (e.g., TLS targets).

3.1. Overview

The kinematic MSS consists of a 3D TLS, a laser tracker, and a special probe (a normally hand-held combination of a reflector and ten LEDs for pose estimation regarding the laser tracker). The TLS is a Zoller + Fröhlich Imager 5016, which is used in a 2D profile mode for this application. The measuring rate of this sensor is 55 profiles/second and its range noise is 0.3 mm for a distance of 10 m [22]. The TLS is mounted on a rollable platform. On the other hand, the laser tracker used is a Leica AT960 LR with its Leica T-Probe. The T-Probe is rigidly mounted on top of the TLS and, therefore, moves with the TLS along the trajectory (cf. Figure 2a). Combination of the laser tracker with the T-probe gives the position with an accuracy of ±15 $\mathsf{\mu }$m + 6 $\mathsf{\mu }$m/m (as a maximum permissible error (MPE) for the 3D position) and the orientation with an MPE of ${0.01}^{\circ }$ = 18 $\mathsf{\mu }$m/100 mm for the accuracy of each orientation-direction, respectively [23]. Due to the integration of such a highly accurate laser tracker into this MSS, the reference pose information with superordinated accuracy could be derived directly.

Data acquisition for this case study was carried out within the basement of the GIH inside a selected section of a corridor (cf. Figure 2b). Characteristics of this section are ideal with extensive walls and obstacles such as pipes below the ceiling and a door in one wall. The kinematic MSS was moved through this environment on an almost linear six meter long trajectory for about 25 s at a slow walking speed. This corresponds to 1311 epochs in total. We were aware that the used trajectory was limited in time and space. However, we intended here only to focus on the applicability of our developed georeferencing approach. The laser tracker was referenced in advance by given control points to ensure transformation to a superordinated coordinate system. The TLS targets regarding this coordinate system are also provided inside the environment measured. They will support further validation of our approach based on 3D point cloud information in the future. However, this issue was not in the focus of this paper. Instead, we will use the highly accurate reference pose by means of the laser tracker for validation. The TLS captured 3D points in a profile mode regarding its local sensor coordinate system. Every full laser scanner profile (LSP) was linked to 6D pose information by means of the laser tracker and T-probe. The right geometrical relation of this 6D pose to the reference point of the TLS was done by means of given calibration parameters (cf. [6]). The kinematic MSS utilized together with all the coordinate systems mentioned are depicted in Figure 2a. Thus, the direction of movement of the MSS is in x-direction. Consequently, the LSPs captured were in the “y–z” plane of the local laser scanner coordinate system. A highly accurate static full 3D laser scan of the captured section of the environment by means of the same laser scanner in 3D mode is also performed for further investigations. An overview of the true trajectory is pictured in Figure 3 as a top view by means of the laser tracker measurements in two different scales for the y-axis.

3.2. Methodology

3.2.1. Observation Vector

The local 2D LSP and 6D pose information by means of the laser tracker in combination with the T-Probe were already at hand by means of the sensor data available from the MSS mentioned in Section 3.1. The 6D pose observations of an IMU are simulated on the basis of such highly accurate reference pose information. For this purpose, noise and a linear drift were added to the reference given to model realistic observations of a moderate and an accurate IMU. This was obviously just a rough approximation and did not reproduce observations of an IMU in reality. However, additional influencing parameters were neglected for the sake of simplicity. Single or rather double integration over a time of 25 s (regarding the duration of measurements mentioned in Section 3.1) for angular velocity and acceleration stability was used. Based on this, it was assumed that a drift in the position of ∼16 m (moderate IMU) or rather ∼2.5 m (accurate IMU) and a drift in the orientation of ∼5${}^{\circ }$ (moderate IMU) or rather ∼0.2${}^{\circ }$ (accurate IMU) was acquired. Due to the lack of information perpendicular to the scanning plane of the laser scanner (in the direction of movement), the position information in the x-direction was not affected by these changes and was consistently equal to a respected reference. The sampling rate of the simulated IMU observations was identical to that of the reference data.

Afterwards, the observation vector ${\mathit{l}}_{\mathit{k}}$, consisting of one local LSP ${\mathit{P}}_{\mathit{k}}^{\mathrm{local}}$ (which consists, in turn, of N single 3D scan points), the 3D position ${\mathit{t}}_{\mathit{k}}$ and 3D rotation matrix ${\mathit{R}}_{\mathit{k}}$ (which is set up based on the three Euler angles ${\Omega }_{\mathit{k}}$, ${\Phi }_{\mathit{k}}$ and ${\mathit{K}}_{\mathit{k}}$) of the IMU are derived for each epoch $\mathit{k}=1\dots \mathit{K}$. Apart from that, the 6D reference pose of the laser tracker (position ${\mathit{t}}_{\mathit{k}}^{*}$ and rotation matrix ${\mathit{R}}_{\mathit{k}}^{*}$) could be relied directly on for the purpose of validation.

$$\begin{array}{c}\hfill {\mathit{l}}_{\mathit{k}}={\left[\underset{{\mathit{P}}_{\mathit{k}}^{\mathrm{local}}}{\underbrace{x_{1,\mathit{k}},{\mathit{y}}_{1,\mathit{k}},{\mathit{z}}_{1,\mathit{k}},\dots ,{\mathit{x}}_{\mathit{N},\mathit{k}},{\mathit{y}}_{\mathit{N},\mathit{k}},{\mathit{z}}_{\mathit{N},\mathit{k}}}},\underset{{\mathit{t}}_{\mathit{k}}}{\underbrace{{\mathit{X}}_{\mathit{k}},{\mathit{Y}}_{\mathit{k}},{\mathit{Z}}_{\mathit{k}}}},\underset{{\mathit{R}}_{\mathit{k}}}{\underbrace{{\Omega }_{\mathit{k}},{\Phi }_{\mathit{k}},{\mathit{K}}_{\mathit{k}}}}\right]}^T.\end{array}$$

(23)

Additionally, the corresponding VCM ${\mathbf{\Sigma }}_{\mathit{vv}}$ of the observations ${\mathit{l}}_{\mathit{k}}$ can be set up, which consists of variances of the IMU ${\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{IMU}}}$ and the quality information of LSP in the form of variances ${\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{LSP}}}$. Related standard deviations for the VCM ${\mathbf{\Sigma }}_{\mathit{vv}}$ are given in

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 }$

. As has already been mentioned, IMU observations in the direction of movement (x) were assumed to be considerably more accurate than the ones in the perpendicular direction (Y and Z). Furthermore, the VCM ${\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{LSP}}}$ applied for LSP was not based directly on the range noise of the laser scanner given by the manufacture. It was concluded in the context of former investigations that such specifications were overoptimistic within the scope of the current approach proposed. This was due to the fact that the observations had to fulfil additional equations (e.g., geometrical constraints) and needed to be more variable. Consequently, standard deviations of the VCM ${\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{LSP}}}$ for LSP were larger than the manufacturer’s specifications and selected generously.

$$\begin{array}{ccc}\hfill {\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{LSP}}}& =\hfill & \hfill diag\left({\sigma }_{{\mathit{x}}_1}^2,{\sigma }_{{\mathit{y}}_1}^2,{\sigma }_{{\mathit{z}}_1}^2,\dots ,{\sigma }_{{\mathit{x}}_{\mathit{N}}}^2,{\sigma }_{{\mathit{y}}_{\mathit{N}}}^2,{\sigma }_{{\mathit{z}}_{\mathit{N}}}^2\right)\end{array}$$

(24)

$$\begin{array}{ccc}\hfill {\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{IMU}}}& =\hfill & \hfill diag\left({\sigma }_{\mathit{X}}^2,{\sigma }_{\mathit{Y}}^2,{\sigma }_{\mathit{Z}}^2,{\sigma }_{\Omega }^2,{\sigma }_{\Phi }^2,{\sigma }_{\mathit{K}}^2\right)\end{array}$$

(25)

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{\mathit{vv}}=\left[\begin{array}{cc}{\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{LSP}}}& \mathit{0}\\ \mathit{0}& {\mathbf{\Sigma }}_{{\mathit{vv}}_{\mathrm{IMU}}}.\end{array}\right]\end{array}$$

(26)

3.2.2. Assignment Algorithm for Distinctive Planes

Further indispensable information were the assignments of every single 3D scan point to distinctive planes (left wall, right wall, ceiling and floor) of the environment. For this purpose, every captured LSP ${\mathit{P}}_{\mathit{k}}^{\mathrm{local}}$ was segmented individually to identify the walls, ceiling and floor properly. This is done by a RANSAC algorithm in order to find suitable line segments within each single LSP. Applied distance threshold for the consensus set was 5 mm in combination with a maximum of 30 iterations. Suitable candidates have a minimum percentage (2%) of points in comparison to the total number of points within the respective LSP. Additionally, at least 20 points needed to be assigned to a line segment. In order to only identify lines, which represented left or right walls or rather ceilings or floors, only those candidates were selected which are almost parallel or perpendicular regarding the standing axis of the laser scanner (which is known by means of the local coordinate system). In order to avoid doors, leads or other obstacles, line candidates were compared regarding averaged assignments of several past LSPs (named as memory subsequently). The criteria used for this are changes in distance between the respective line and origin of the laser scanner and the variation of the averaged intensity of respective scan points. Both criteria are analyzed regarding the memory mentioned. Rough outliers in the assignment could be identified by applying such a restriction. Finally, every N single 3D scan point of the LSP ${\mathit{P}}_{\mathit{k}}^{\mathrm{local}}$ within each epoch $\mathit{k}=1\dots \mathit{K}$ is assigned to left wall, right wall, ceiling, floor or remains as unused. These extended LSP are denoted ${\mathit{C}}_{\mathit{k}}^{\mathrm{local}}$ subsequently. Thus, in total, ${\mathit{C}}_{\mathit{k}}^{\mathrm{local}}$ is equal to ${\mathit{P}}_{\mathit{k}}^{\mathrm{local}}$ but contains mentioned additional segmentation information for every measured scan point. The results of the assignment algorithm introduced in relation to the case study are depicted in Figure 4a,b. Interfering objects (e.g., pipes and cables) are erased.

3.2.3. Measurement Equation and State Parameter Vector

The state parameters desired were inter alia, relative changes in 3D position $\Delta {\mathit{t}}_{\mathit{k}}$, 3D orientation $\Delta {\mathit{R}}_{\mathit{k}}\left(\Delta {\Omega }_{\mathit{k}},\Delta {\Phi }_{\mathit{k}},\Delta {\mathit{K}}_{\mathit{k}}\right)$ and 3D velocity $\Delta {\mathit{v}}_{\mathit{k}}$. In combination with the (simulated) noisy and drifted IMU pose $({\mathit{t}}_{\mathit{k}},{\mathit{R}}_{\mathit{k}})$ they ended up in the almost true position ${\mathit{t}}_{\mathit{k}}^{\mathrm{MSS}}$ and orientation ${\mathit{R}}_{\mathit{k}}^{\mathrm{MSS}}\left({\Omega }_{\mathit{k}}^{\mathrm{MSS}},{\Phi }_{\mathit{k}}^{\mathrm{MSS}},{\mathit{K}}_{\mathit{k}}^{\mathrm{MSS}}\right)$ of the MSS at each epoch $\mathit{k}$:

$$\begin{array}{c}\hfill {\mathit{t}}_{\mathit{k}}^{\mathrm{MSS}}={\mathit{t}}_{\mathit{k}}+\Delta {\mathit{t}}_{\mathit{k}},{\mathit{R}}_{\mathit{k}}^{\mathrm{MSS}}=\Delta {\mathit{R}}_{\mathit{k}}·{\mathit{R}}_{\mathit{k}}.\end{array}$$

(27)

This formulation of relative changes as states might evoke a relation towards an error-state KF (ErKF, or indirect KF) [24,25]. However, the underlying concept of an ErKF is different. Instead of direct relative measurements (e.g., from an IMU), we included laser scanner observations by means of an implicit formulation. Georeferencing of every local LSP ${\mathit{P}}_{\mathit{k}}^{\mathrm{local}}$ regarding the superordinated coordinate system ${\mathit{P}}_{\mathit{k}}^{\mathrm{global}}$ can be applied by transformation using the estimated pose of the MSS:

$$\begin{array}{c}\hfill {\mathit{P}}_{\mathit{k}}^{\mathrm{global}}=\left({\mathit{t}}_{\mathit{k}}+\Delta {\mathit{t}}_{\mathit{k}}\right)+\left(\Delta {\mathit{R}}_{\mathit{k}}·{\mathit{R}}_{\mathit{k}}\right)·{\mathit{P}}_{\mathit{k}}^{\mathrm{local}}.\end{array}$$

(28)

At this point, prior information is integrated into our approach. Several geometrical circumstances could be taken into consideration during the movement through the corridor (cf. Figure 2b). In the current case, it is assumed that certain parts of all individual LSP’s captured some random parts of the left wall, right wall, ceiling and floor of the environment. Within a certain region, it could also be presumed that respective detected points on the left wall, right wall, ceiling and floor each refer to the same geometrical planes, respectively.

By using such information, the measurement equation could be formulated by means of the well-known Hesse normal form of a plane:

$$\begin{array}{c}\hfill {\mathit{n}}_{\mathit{e}}·\underset{{\mathit{C}}_{\mathit{k}}^{\mathrm{global}}}{\underbrace{\left[\left({\mathit{t}}_{\mathit{k}}+\Delta {\mathit{t}}_{\mathit{k}}\right)+\left(\Delta {\mathit{R}}_{\mathit{k}}·{\mathit{R}}_{\mathit{k}}\right)·{\mathit{C}}_{\mathit{k}}^{\mathrm{local}}\right]}}-{\mathit{d}}_{\mathit{e}}=0,\end{array}$$

(29)

where ${\mathit{n}}_{\mathit{e}}$ is the $3\times 1$ normal vector of the left wall (or rather right wall, ceiling, floor) and ${\mathit{d}}_{\mathit{e}}$ the related distance to the origin. Additionally, the segmented LSP information mentioned ${\mathit{C}}_{\mathit{k}}^{\mathrm{local}}$ regarding the left and right wall, ceiling and floor of the environment (cf. Section 3.2.2) could also be taken into consideration. In relation to Section 2.1 the given overall measurement Equation in (29) has an implicit formulation of type $\mathit{h}(\mathit{l},\mathit{x})=\mathbf{0}$.

Hence, the 25-dimensional state parameter vector ${\mathit{x}}_{\mathit{k}}$ could be set up by means of the relative changes requested in position $\Delta {\mathit{t}}_{\mathit{k}}$, orientation $\Delta {\mathit{R}}_{\mathit{k}}\left(\Delta {\Omega }_{\mathit{k}},\Delta {\Phi }_{\mathit{k}},\Delta {\mathit{K}}_{\mathit{k}}\right)$ and velocity $\Delta {\mathit{v}}_{\mathit{k}}$ and four sets of plane parameters with each four parameters ${\mathit{n}}_{{\mathit{e}}_{\mathit{x}}},{\mathit{n}}_{{\mathit{e}}_{\mathit{y}}},{\mathit{n}}_{{\mathit{e}}_{\mathit{z}}},{\mathit{d}}_{\mathit{e}}$. Here, $\mathit{e}$ can stand for the left wall (or rather right wall, ceiling or floor):

$$\begin{array}{c}\hfill {\mathit{x}}_{\mathit{k}}={\left[\underset{\Delta {\mathit{t}}_{\mathit{k}}}{\underbrace{\Delta X_{\mathit{k}},\Delta Y_{\mathit{k}},\Delta Z_{\mathit{k}}}},\underset{\Delta {\mathit{R}}_{\mathit{k}}}{\underbrace{\Delta {\Omega }_{\mathit{k}},\Delta {\Phi }_{\mathit{k}},\Delta {\mathit{K}}_{\mathit{k}}}},\underset{\Delta {\mathit{v}}_{\mathit{k}}}{\underbrace{\Delta v_{x_{\mathit{k}}},\Delta v_{y_{\mathit{k}}},\Delta v_{z_{\mathit{k}}}}},\underset{leftwall}{\underbrace{{\mathit{n}}_{{\zeta }_{\mathit{x}}},{\mathit{n}}_{{\zeta }_{\mathit{y}}},{\mathit{n}}_{{\zeta }_{\mathit{z}}},{\mathit{d}}_{\zeta }}},\dots ,\underset{floor}{\underbrace{{\mathit{n}}_{{\xi }_{\mathit{x}}},{{n}}_{{\xi }_{\mathit{y}}},{\mathit{n}}_{{\xi }_{\mathit{z}}},{\mathit{d}}_{\xi }}}\right]}^T.\end{array}$$

(30)

It is worth mentioning that the increase of epochs in trajectories is associated with the increase of geometric details (e.g., walls) of buildings in the environment within real world application. This leads to an unlimited expansion of the state vector. The usage of a dual state Kalman filter (DKF) in such a case might be suitable. This would enable strict separation of time changing states (e.g., position, orientation, velocity) and other over time static parameters (e.g., normal vector and distances to origin of a plane) [26]. However, interaction of DKF and implicit measurement equations was not treated in this paper.

3.2.4. System Equation

A simple physical model was used to predict the constraint states from previous epoch $\mathit{k}-1$ to the current $\mathit{k}$. This state transition was based on a constant velocity model, which only affected the six pose parameters and three velocities of the state parameter vector [27]. All plane parameters were unaffected by this prediction step and were equal to the constraint state ${\tilde{\mathit{x}}}_{\mathit{k}-1}^{+}$:

$$\begin{array}{cc}\hfill \Delta {\widehat{\mathit{t}}}_{\mathit{k}}^{-}=& \hfill \Delta {\widehat{\mathit{t}}}_{\mathit{k}-1}^{+}+\Delta {\widehat{\mathit{v}}}_{\mathit{k}-1}^{+}·\Delta \tau +{\mathit{w}}_{\Delta \mathit{t},\mathit{k}-1}\end{array}$$

(31)

$$\begin{array}{cc}\hfill \Delta {\widehat{\mathit{R}}}_{\mathit{k}}^{-}=& \hfill \Delta {\widehat{\mathit{R}}}_{\mathit{k}-1}^{+}+{\mathit{w}}_{\Delta \mathit{R},\mathit{k}-1}\end{array}$$

(32)

$$\begin{array}{cc}\hfill \Delta {\widehat{\mathit{v}}}_{\mathit{k}}^{-}=& \hfill \Delta {\widehat{\mathit{v}}}_{\mathit{k}-1}^{+}+{\mathit{w}}_{\Delta \mathit{v},\mathit{k}-1}\end{array}$$

(33)

$$\begin{array}{cc}\hfill {\widehat{\mathit{n}}}_{\mathit{e},\mathit{k}}^{-}=& \hfill {\widehat{\mathit{n}}}_{\mathit{e},\mathit{k}-1}^{+}\end{array}$$

(34)

$$\begin{array}{cc}\hfill {\widehat{\mathit{d}}}_{\mathit{e},\mathit{k}}^{-}=& \hfill {\widehat{\mathit{d}}}_{\mathit{e},\mathit{k}-1}^{+},\end{array}$$

(35)

where ${\mathit{w}}_{\Delta \mathit{t},\mathit{k}-1}$, ${\mathit{w}}_{\Delta \mathit{R},\mathit{k}-1}$ and ${\mathit{w}}_{\Delta \mathit{v},\mathit{k}-1}$ are the process noise vectors and $\Delta \tau$ is the time interval between two consecutive epochs. The VCM of the process noise ${\mathbf{\Sigma }}_{\mathit{ww}}$ represents related system noise during the prediction step. Due to simplicity, all variances and covariances were zero, except for the process noise of the velocity. Within this case study a definition of ${\sigma }_{\mathit{v},\mathit{w}}=5·\Delta \tau$ is selected.

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{\mathit{ww}}=\left[\begin{array}{cc}diag\left({\mathbf{0}}_{[1\times 6]},\right.{\sigma }_{\mathit{v},\mathit{w}}^2,{\sigma }_{\mathit{v},\mathit{w}}^2,{\sigma }_{\mathit{v},\mathit{w}}^2& \mathit{0}\\ \mathit{0}& diag\left({\mathbf{0}}_{[1\times 16]}\right)\end{array}\right].\end{array}$$

(36)

3.2.5. Nonlinear Equality and Inequality Constraint for the State Parameters

In addition to the measurement Equation (29), the geometric prior information by means of equality and inequality constraints is also used to improve the georeferencing of the MSS. Due to the fact that the plane parameters ${\mathit{n}}_{\mathit{e}}$ within the state parameter vector ${\mathit{x}}_{\mathit{k}}$ were used, the unity of normal vectors had to be ensured. In order to do so, nonlinear equality constraints can be used:

$$\begin{array}{c}\hfill \mathit{g}\left({\mathit{x}}_{\mathit{k}}\right)=\left|\right|{\mathit{n}}_{\mathit{e}}\left|\right|=\sqrt{{\mathit{n}}_{{\mathit{e}}_{\mathit{x}}}^2+{\mathit{n}}_{{\mathit{e}}_{\mathit{y}}}^2+{\mathit{n}}_{{\mathit{e}}_{\mathit{z}}}^2}=b=1.\end{array}$$

(37)

Furthermore, inequality constraints regarding intersection angles of related planes are also implemented. In this context, obvious conditions for concurrency and perpendicularity between distinctive walls are relied on. It would also be possible to formulate these constraints by means of equality constraints. However, instead of using such hard constraints, the use of inequality constraints together with lower ${\mathit{lb}}_{\mathit{i},\mathit{k}}$ and upper ${\mathit{ub}}_{\mathit{i},\mathit{k}}$ boundaries (cf. Section 2.2) are preferred. Applying such inequality constraints is more consistent with reality, where such perfect conditions are rather rare or can be rarely fulfilled. Selected thresholds for this are derived based on documented standards for the building industry [28] and should be stated around $0^{\circ }$ (for concurrency) and ${90}^{\circ }$ (for perpendicularity). As a further basis, the information based on the highly accurate static 3D laser scanner point cloud mentioned are used to set up the boundaries. By means of this reference, true intersection angles between walls can be determined and applied. Consequently, the boundaries are selected by considering ${0.5}^{\circ }$ for the intersection angles mentioned:

$$\begin{array}{c}\hfill {\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)=co{s}^{-1}\left(\frac{|{\mathit{n}}_{\zeta }·{\mathit{n}}_{\xi }|}{|{\mathit{n}}_{\zeta }|·|{\mathit{n}}_{\xi }|}\right)=co{s}^{-1}\left(\frac{|{\mathit{n}}_{{\zeta }_{\mathit{x}}}{\mathit{n}}_{{\xi }_{\mathit{x}}}+{\mathit{n}}_{{\zeta }_{\mathit{y}}}{\mathit{n}}_{{\xi }_{\mathit{y}}}+{\mathit{n}}_{{\zeta }_{\mathit{z}}}{\mathit{n}}_{{\xi }_{\mathit{z}}}|}{\sqrt{{\mathit{n}}_{{\zeta }_{\mathit{x}}}^2+{\mathit{n}}_{{\zeta }_{\mathit{y}}}^2+{\mathit{n}}_{{\zeta }_{\mathit{z}}}^2}·\sqrt{{\mathit{n}}_{{\xi }_{\mathit{x}}}^2+{\mathit{n}}_{{\xi }_{\mathit{y}}}^2+{\mathit{n}}_{{\xi }_{\mathit{z}}}^2}}\right)\end{array}$$

(38)

$$\begin{array}{c}\hfill {\mathit{lb}}_{\mathit{i},\mathit{k}}\le {\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)\le {\mathit{ub}}_{\mathit{i},\mathit{k}}\mathrm{with}:{\mathit{lb}}_{\mathit{i},\mathit{k}}={\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)-{0.5}^{\circ },{\mathit{ub}}_{\mathit{i},\mathit{k}}={\mathit{g}}_{\mathit{i}}\left({\mathit{x}}_{\mathit{k}}\right)+{0.5}^{\circ }.\end{array}$$

(39)

Due to the geometrical behaviour of the environment, there are several possibilities to apply Equation (38) in terms of concurrency or rather perpendicularity. For this reason, there are also different options for the number of geometrical inequality constraints selected. Within Section 3.3, the respective impacts and benefits of combining several constraints in contrast to individual use cases are shown. Regardless of the respective combination, which constraints are active within each epoch should be checked. This means that constraints can only be applied if at least five points of the related walls, ceiling or floor are segmented within this epoch. If there is a lack of one or several walls, all respective constraints to this wall will be inactive for this epoch.

3.2.6. Initialization

Initialization of approximate values for the state vector ${\mathit{x}}_0$ and the related VCM ${\mathbf{\Sigma }}_{\mathit{xx},0}$ are needed to perform the IEKF. Initial relative changes in position $\Delta {\mathit{t}}_0$ and orientation $\Delta {\mathit{R}}_0$ are selected by means of the difference between the reference pose and IMU regarding first epoch $k=0$. Relative velocities $\Delta {\mathit{v}}_0$ are initialized as zero. Initial values for the normal vectors of the planes ${\mathit{n}}_{\mathit{e},0}$, are estimated by means of the first LSP and its respective points for left wall, right wall, ceiling and floor. Related standard deviations for the VCM ${\mathbf{\Sigma }}_{\mathit{xx},0}$ are given in

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

.

$$\begin{array}{c}\hfill {\mathit{x}}_0={\left[\Delta X_0,\Delta Y_0,\Delta Z_0,\Delta {\Omega }_0,\Delta {\Phi }_0,\Delta {\mathit{K}}_0,\Delta v_{x_0},\Delta v_{y_0},\Delta v_{z_0},{\mathit{n}}_{{\zeta }_{\mathit{x}},0},{\mathit{n}}_{{\zeta }_{\mathit{y}},0},{\mathit{n}}_{{\zeta }_{\mathit{z}},0},{\mathit{d}}_{\zeta ,0},\dots ,{\mathit{n}}_{{\xi }_{\mathit{x}},0},{\mathit{n}}_{{\xi }_{\mathit{y}},0},{\mathit{n}}_{{\xi }_{\mathit{z}},0},{\mathit{d}}_{\xi ,0}\right]}^T\end{array}$$

(40)

$$\begin{array}{c}\hfill {\mathbf{\Sigma }}_{\mathit{xx},0}=diag\left({\sigma }_{\Delta X}^2,{\sigma }_{\Delta Y}^2,{\sigma }_{\Delta Z}^2,{\sigma }_{\Delta \Omega }^2,{\sigma }_{\Delta \Phi }^2,{\sigma }_{\Delta \mathit{K}}^2,{\sigma }_{\Delta v_x}^2,{\sigma }_{\Delta v_y}^2,{\sigma }_{\Delta v_z}^2,{\sigma }_{{\mathit{n}}_{{\zeta }_{\mathit{x}}}}^2,{\sigma }_{{\mathit{n}}_{{\zeta }_{\mathit{y}}}}^2,{\sigma }_{{\mathit{n}}_{{\zeta }_{\mathit{z}}}}^2,{\sigma }_{{\mathit{d}}_{\zeta }}^2,\dots ,{\sigma }_{{\mathit{n}}_{{\xi }_{\mathit{x}}}}^2,{\sigma }_{{\mathit{n}}_{{\xi }_{\mathit{y}}}}^2,{\sigma }_{{\mathit{n}}_{{\xi }_{\mathit{z}}}}^2,{\sigma }_{{\mathit{d}}_{\xi }}^2\right).\end{array}$$

(41)

3.3. Results

In order to ensure independence from simulated IMU pose information, the results within this Section 3.3 are, with respect to the mean of 500 replications, of slightly different realizations of the IMU pose information. Additionally, to investigate the differences with respect to a moderate and an accurate IMU, results of two sets of simulations are presented within this Section 3.3. A schematic overview of this procedure is depicted in Figure 5. Evaluation is done by means of the estimated pose parameters of the kinematic MSS ${\mathit{t}}_{\mathit{k}}^{\mathrm{MSS}}$ and ${\mathit{R}}_{\mathit{k}}^{\mathrm{MSS}}$ and the ground truth by means of the laser tracker ${\mathit{t}}_{\mathit{k}}^{\mathrm{GT}}$ and ${\mathit{R}}_{\mathit{k}}^{\mathrm{GT}}$. Based on these pose information, the root mean square error (RMSE) for the combined position in the x-, y-, z-direction can be calculated (cf. (42)). In order to give a quality parameter for combined orientation, transformation from the rotation matrix ${\mathit{R}}_{\mathit{k}}$ to the axis-angle representation by a normalized vector ${\mathit{r}}_{\mathit{k}}=\left[{\mathit{r}}_1,{\mathit{r}}_2,{\mathit{r}}_3\right]$ and rotation angle ${\Theta }_{\mathit{k}}$ is performed. Afterwards, the mean error (ME) of the representative angle between estimation ${\Theta }_{\mathit{k}}^{\mathrm{MSS}}$ and ground truth ${\Theta }_{\mathit{k}}^{\mathrm{GT}}$ is calculated and used (cf. (43)). Presentation of the results by means of combined position and orientation instead of a single axis is intended. In such a manner, we can identify the most suitable combination of state constraints for this approach while keeping the results clear.

$$\begin{array}{ccc}\hfill RMSE& =\hfill & \hfill \sqrt{\frac{1}{k}{\sum }_{i=1}^k\left({\left({\mathit{X}}_{\mathit{k}}^{\mathrm{GT}}-{\mathit{X}}_{\mathit{k}}^{\mathrm{MSS}}\right)}^2+{\left({\mathit{Y}}_{\mathit{k}}^{\mathrm{GT}}-{\mathit{Y}}_{\mathit{k}}^{\mathrm{MSS}}\right)}^2+{\left({\mathit{Z}}_{\mathit{k}}^{\mathrm{GT}}-{\mathit{Z}}_{\mathit{k}}^{\mathrm{MSS}}\right)}^2\right)}\end{array}$$

(42)

$$\begin{array}{ccc}\hfill ME& =\hfill & \hfill \frac{1}{k}\sum _{i=1}^k\left(|{\Theta }_{\mathit{k}}^{\mathrm{GT}}-{\Theta }_{\mathit{k}}^{\mathrm{MSS}}|\right).\end{array}$$

(43)

The difference between both classes of IMUs, as well as ground truth, is shown in Figure 6 by means of their averaged change in position and orientation over all corresponding 500 replications. As it has already been mentioned in Section 3.2.1, position in the x-direction for both IMUs was identical to the ground truth by means of the laser tracker. However, a major linear drift is visible (∼15 m for moderate IMU or rather ∼2.5 m for accurate IMU) for position in both other directions. Due to different assumed uncertainties for both IMUs, the drift in orientation for the accurate IMU was rather negligible, whereas the drift for the moderate IMU was about ∼5${}^{\circ }$ for all axes.

Based on this IMU pose information, methods from Section 3.2 are applied. In addition to both classes of IMUs, combinations of respective equality and inequality state constraints also affects the pose parameters requested. In the framework of this paper, estimates of ten different combinations (listed in

Table 3. Investigated combinations of respective equality (red) and inequality (green) state constraints. Applied constraints within each combination are depicted with a $✓$ 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						✓				✓

) are presented. In order to compare various constraints in the developed IEKF algorithm, the combination I was designed without using any constraints and will be considered as a reference solution. All other combinations (II–X) relied on different equality and inequality constraints which include concurrency and/or perpendicularity between assigned left/right wall or rather ceiling and floor. In all these combinations the constraints regarding normal plane vectors (cf. (37)) were formulated in order to ensure numerical and geometrical stability. The inequality constraints in combinations III–VI were applied independently from each other whereas in combination VII–X a collaboration between concurrency and perpendicularity was enabled in order to evaluate respective impact on the state estimates for each collaboration. However, it is worth mentioning that the impact of the individual combinations might vary depending on respective application and related environmental circumstances.

The results achieved over all 500 replications for both moderate and accurate IMU observations in relation to the ten different combinations of applied state constraints are summarized in

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$·{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

for combined position by means of RMSE and

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

for combined orientation by means of ME. Comparison between the results was determined by means of minimum (min), maximum (max), mean, median and standard deviation (SD), as well as lower bound (↓) and upper bound (↑) of the $95\%$ confidence interval (CI), calculated numerically from the 500 samples, as selected characteristic values.

It is notable that, independently from the IMU used, pose estimation fails if no constraints (combination I) are applied. It further stands out that there was an impact of the RMSE and ME depending on the constraints applied. In terms of position, combination III delivers the lowest estimates for both IMUs. Whereas for the moderate IMU, combination X was the lowest in terms of orientation, and for the accurate IMU, combination III is also the lowest (both judged by median). However, without taking into account combination I, all solutions by means of the applied state constraints were smaller than the ME for orientation of the moderate IMU. For the accurate IMU only combinations III, V, VI and VIII were smaller than the noisy and drifted IMU solution. However, the gain in accuracy is much higher for the position compared to the orientation.

Due to the conclusions provided by means of Table 4 and Table 5, the temporal progress in position and orientation of the RMSE or rather ME for different combinations of state constraints are depicted in Figure 7 for the moderate IMU. The same results regarding the accurate IMU are depicted in Figure 8. For presentation purposes, inaccurate solutions (e.g., IMU in terms of position; combination I) are omitted. The basic behaviour of the temporal progress of the RMSE for position of both IMUs was very similar. All combinations increased drastically within the very first epochs. After this running-in effect of the filter they decrease quickly and continue differently over time. Over all epochs, combination II leads to a significant larger RMSE and has the largest increase. This is of interest, except normalized plane normal vectors, no further geometrical constraints like concurrency or perpendicularity were considered within this configuration. Combination III, V and VIII are very similar and lead to the best results around 1.5 cm. Remaining combinations have a slightly larger increase and will end up between 2–5 cm. Temporal behaviour of the ME for orientation is slightly different for both IMUs. They also increased drastically within the very first epochs. Afterwards, the gradient was related to the initially drifted IMU solution. However, all presented solutions for the moderate IMU were lower than respective initial IMU solution. In addition, gradient and progress are almost identical for this type of IMU. In case of the accurate IMU, there was a slight variation between all combinations. But from epoch k = 800 the increase was for all combinations lower than the IMU solution. Combination III behaves most similar to the IMU solution in the beginning and undercut the IMU curve at epoch k = 400.

In order to investigate the individual best results for position and orientation, respective histograms regarding the related 500 replications are depicted in Figure 9 for the moderate and accurate IMU with respect to each other. Based on these representations, further conclusions can be drawn. All histograms show distributions which are right-skew symmetric. This indicates that, independent from the IMU observation applied, there are a few configurations within the respective 500 replications which lead to a slightly larger RMSE or rather ME or even outliers. However, this skew is much more pronounced in case of the RMSE for position. The histograms for the ME of the orientation are similar to a Gaussian distribution. This different behaviour is not directly explainable and further investigations are needed. For this reason, a more detailed arrangement regarding the single coordinate axis in contrast to the combined presentation appears appropriate and will be realized in the future.

In general, it can be summarized that the consideration of state constraints improved state estimation significantly. However, differences between individual combinations were quite small. For this reason, geometrical restrictions regarding perpendicularity and concurrency depend strongly on respective environments.

4. Discussion

The results presented in Section 3.3 indicate significant dependencies of the estimated pose parameters on the respective equality and inequality state constraints applied. Moreover, no prominent combination of constraints exists which fits to all requirements in terms of position and orientation. The two different types of IMU observations demonstrate additional dependencies. Depending on the respective accuracy class, the use of certain constraints can significantly improve pose estimation. This applies particularly to the orientation estimation, whereas position estimation benefits from almost every constraint applied, although to different levels. Overall, it could be seen that the usage of state constraints results in an important added value. However, there is an important need to define and apply a suitable model selection procedure into the current filter approach. Various different constraint combinations are applied and such a procedure should determine which combination is most suitable. Our priors will be obtained by considering which constraints are representative of features we expect to see in the data, and which would produce biased or inaccurate estimates. In addition, the effects of different individual constraints, in contrast to combined constraints, will be investigated more extensively in the future. Possible linear dependencies between individual constraints need to be analyzed and, if necessary, neglected.

As it has already been mentioned in Section 2.1, the compatibility of implicit measurement equations and state constraints as part of the IEKF is an important issue which needs further consideration. Due to implicit formulation, both states and observations are corrected within the update step to fulfill the measurement equation. During the constraint step, only the states are affected, while observations are unaffected. Consequently, this leads to a violated measurement equation. For this reason, the constraint step is going to be directly integrated into the update step of the IEKF. In terms of equality constraints, extension of the objective function should be sufficient. However, in terms of inequality constraints, this is not possible straightforwardly. Combination of inequality state constraints in the scope of GHM is treated in [29] and shows the complexity relating thereto. Another more promising approach for this task will be to use soft constraints instead of inequality constraints, which will be investigated in the future. Also, consideration of perfect measurements is possible, however, this would only allow equality constraints. In theory, state constraints can also be applied in combination with other classes of Kalman filters (e.g., UKF) to avoid the factual linearization issues of the EKF and IEKF. However, compatibility of implicit measurement equations besides linear KF, EKF and IEKF need to be solved first.

The dependency mentioned regarding the IMU observations applied is an argument to consider initial biases and drift parameters of the IMU as additional state parameters within further developments. By doing so, direct estimation and consideration within the IEKF are possible and should further enhance the pose estimation. As a consequence of such an extension, this would be accompanied by further development of a more suitable system model within the prediction step.

5. Conclusions

We presented a novel method to consider nonlinear equality and inequality state constraints within the framework of an IEKF with implicit measurement equations. Consideration of such restrictions is realized by means of flexible PDF truncation. This method was applied and evaluated for georeferencing of an indoor laser scanner-based kinematic MSS. Therefor, different combinations of geometrical constraints were applied for real measurement data.

In conclusion, the consideration of appropriate restrictions between the state parameters is desirable. The use of inequality constraints in addition to equality constraints offers further possibilities in terms of accuracy. This justifies general consideration of inequality state constraints for georeferencing of a kinematic MSS.

Furthermore, adaptation and application of the general georeferencing approach by means of an IEKF with respect to other kinematic MSS is planned to verify its general validity. The focus there is to apply the approach on an UAV and an outdoor mobile mapping system. Both applications require special demands concerning 3D point cloud assignments in terms of facades, building models and further external influences which may occur within outdoor environments. In addition, the approach presented in this article has to be applied for longer data sets (with respect to spatial and temporal expansion). For this, it is assumed that the RMSE and ME will increase slightly over time unless absolute landmarks are integrated at certain points in time or assumed geometrical constraints are not applicable. Also nonlinear trajectories with turning manoeuvres need to be considered. This might make it necessary to introduce new planes and respective parameters into the model. However, applicability will be ensured as long as sufficient additional information from object space are available, assignable and applicable.

In addition, a more simple application example might be suitable to evaluate a comparison of different constraint combinations and methods to apply them to the fundamental algorithm of the IEKF with implicit measurement equations. As mentioned in Section 2, also other methods can be applied to consider state constraints. While PDF truncation provides great flexibility in simultaneously applying equality and inequality constraints, other methods mentioned above may be more appropriate (e.g., with respect to the uncertainty of the estimated state parameters and the computing time of the algorithm). However, methods for inequality constraints are limited as long as quadratic programming problems should be avoided.