## Abstract

In engineering geodesy, the technical progress leads to various kinds of multi-sensor systems (MSS) capturing the environment. Multi-sensor systems, especially those mounted on unmanned aerial vehicles, subsequently called unmanned aerial system (UAS), have emerged in the past decade. Georeferencing for MSS and UAS is an indispensable task to obtain further products of the data captured. Georeferencing comprises at least the determination of three translations and three rotations. The availability and accuracy of Global Navigation Satellite System (GNSS) receivers, inertial measurement units, or other sensors for georeferencing is not or not constantly given in urban scenarios. Therefore, we utilize UAS-based laser scanner measurements on building facades. The building latter are modeled as planes in a three-dimensional city model. We determine the trajectory of the UAS by combining the laser scanner measurements with the plane parameters. The resulting implicit measurement equations and nonlinear equality constraints are covered within an iterated extended Kalman filter (IEKF). We developed a software simulation for testing the IEKF using different scenarios to evaluate the functionality, performance, strengths, and remaining challenges of the IEKF implemented.

## Zusammenfassung

*Georeferenzierung von Unmanned Aerial Systems mit Hilfe eines iterativen erweiterten Kalman Filters und eines 3D Gebäudemodells*. In der Ingenieurgeodäsie führt der technische Fortschritt zu verschiedenen Arten von Multisensorsystemen (MSS), die der Erfassung der Umgebung dienen. In der vergangenen Dekade sind sehr viele MSS hinzugekommen, die auf einem Unmanned Aerial Vehicle montiert wurden. Diese MSS werden nachfolgend als Unmanned Aerial Systems (UAS) bezeichnet. Die Georeferenzierung von MSS und UAS ist ein notwendiger Schritt zur weiteren Datenverarbeitung. Die Georeferenzierung beinhaltet mindestens die Bestimmung von drei Translationen und drei Rotationen. Die erforderlichen Daten aus GNSS-Empfängern, inertialen Messsystemen oder anderen Sensoren zur Georeferenzierung sind in urbanem Umfeld nicht immer lückenlos und mit der erforderlichen Genauigkeit verfügbar. Deshalb werden in diesem Ansatz die Messungen UAS-basierter Laserscanner auf Gebäudefassaden verwendet. Letztere sind als Ebenen in einem 3D-Gebäudemodell modelliert. Die Trajektorie des UAS wird durch Kombination der Laserscanner-Messungen mit den Ebenenparametern ermittelt. Die daraus resultierenden impliziten Beobachtungsgleichungen und die nichtlinearen Restriktionsgleichungen werden innerhalb eines iterativen erweiterten Kalman-Filters (IEKF) modelliert. Außerdem wurde eine Softwaresimulation für den Test des IEKF entwickelt, um mit verschiedenen Szenarien die Funktionalität, Leistungsfähigkeit und verbleibende Herausforderungen zu bewerten.

## Introduction

It is possible nowadays using the increasing capability and ongoing miniaturization of hardware and sensors to construct various kinds of multi-sensor systems (MSS). The MSS are frequently used in engineering geodesy to capture the environment. It is crucial to know the position and orientation of the MSS with respect to a superordinate coordinate system^{Footnote 1} for a further processing of the data captured. The task to determine position and orientation in a superordinate coordinate system is called georeferencing. Georeferencing usually comprises the determination of three translations and three rotations, also known as six Degrees of Freedom (DoF) or pose.

Unmanned aerial vehicles (UAV) have become a widespread, useful and affordable platform for the MSS in the past decade, optimally suited to capture common scenarios in engineering geodesy. The MSS mounted on a UAV are subsequently called unmanned aerial systems (UAS).

Precise georeferencing of a UAS is a challenging task, especially in urban areas. Global Navigation Satellite System (GNSS) data are affected by shadowing and multipath effects and are, therefore, inaccurate or even unavailable. Basically, low-cost GNSS receivers obtain positional accuracies of a few meters. These accuracies may be improved to decimeter or even centimeter level under good GNSS conditions and using satellite-based augmentation systems, differential GNSS, or real-time kinematics. Lightweight and low-cost inertial measurement units (IMU)^{Footnote 2} obtain orientation accuracies of \(0.1^{\circ}\) for roll and pitch and slightly worse accuracy, \(0.8^{\circ}\), for heading. Combined GNSS-IMU systems improve the heading accuracy to 0.2°–\(0.5^{\circ}\). However, a general problem with IMU data is that it is often seriously affected by drifts.

A further possibility for georeferencing of an MSS is tracking using an external sensor. This is a cumbersome and inefficient task in the case of georeferencing a UAS due to limited measuring ranges and occultations.

In this paper, we present the georeferencing of a UAS, which is equipped with a three-dimensional (3D) laser scanner, a low-cost GNSS receiver, and an IMU, among other sensors. We set ourselves the requirement to determine the pose of the UAS with a precision (as a measure of the accuracy) better than 10 cm in position and \(0.1^{\circ}\) for orientation. The accuracy of the pose is decisive for the further results derived, such as digital terrain models or detailed 3D city models. Therefore, we fused the UAS-based laser scanner measurements towards building facades, which are modeled in a 3D city model, and the other sensor data within an iterated extended Kalman filter (IEKF). We developed a software simulation for testing the IEKF developed using different scenarios to evaluate the functionality and performance of the IEKF implemented. All results presented are part of a research project, subsequently called the *UAS project*.

### Georeferencing of MSS/UAS

The strategies to accomplish the georeferencing vary depending on the measurement configuration, the sensors and hardware available, the environment, and the accuracies to be fulfilled. The georeferencing strategies can be generally classified into *direct*, *indirect*, and *data-driven* georeferencing (see, e.g., Schuhmacher and Böhm 2005; Paffenholz 2012; Holst et al. 2015). An alternative classification regarding indoor applications is given in Vogel et al. (2016).

For *direct*^{Footnote 3} georeferencing, sensors which measure the pose of an MSS directly are integrated into the MSS. These sensors could be, for example, a GNSS receiver (Paffenholz 2012; Talaya et al. 2004), an IMU (Talaya et al. 2004), or an external sensor, such as a total station or a laser tracker (Dennig et al. 2017; Hartmann et al. 2018), which determine the pose by angle and distance measurements to a reflector on the MSS.

*Indirect*^{Footnote 4} georeferencing comprises methods where the pose of the MSS is determined by measurements towards known targets. These targets may be flat markers with a specific pattern (Abmayr et al. 2008) or simple 3D geometries, such as cylinders or spheres (Elkhrachy and Niemeier 2006). The position of the targets within the superordinate coordinate system is determined using an external sensor, for example, a total station. Indirect georeferencing is commonly used in bundle adjustment or for the georeferencing of static terrestrial laser scanners (TLS).

*Data-driven* georeferencing conforms basically to the indirect georeferencing. Instead of known targets, the data sets are matched to reference data sets. These reference data sets may be point clouds georeferenced already (Soloviev et al. 2007; Glira et al. 2015), digital surface models or 3D city models (Hebel et al. 2009; Li-Chee-Ming and Armenakis 2013; Unger et al. 2016, 2017). The matching is accomplished, for instance, via point-to-point assignment, for example, an iterative closest point algorithm (Besl and McKay 1992) or point-to-object assignment (see, e.g., Schuhmacher and Böhm 2005). Please note that *data-driven* georeferencing approaches vary widely. A huge part can be found in the commonly used Simultaneous Localization and Mapping (SLAM) approaches (see, e.g., Nguyen et al. 2006; Lee et al. 2007; Jutzi et al. 2013; Kaul et al. 2015; Nüchter et al. 2015).

In principle, all the georeferencing strategies mentioned are suitable for georeferencing static and kinematic MSS. For a static MSS, the six DoF have to be determined only once, whereas for a kinematic MSS, the six DoF for each measuring epoch should be determined. It may be indispensable to model the pose almost continuously depending on the measuring frequency of the sensors used.

### Filtering Techniques for Georeferencing

Georeferencing of a kinematic MSS is commonly covered as part of the state parameter vector within a filtering approach. Filtering is a two-step procedure in which the current state parameter vector is estimated based on the previous state parameter vector and the current observations. In the first step, called prediction, the current state parameter vector is predicted using the previous state parameter vector and a specific system model. The system model takes into account all the controlling variables and environmental noises that affect the current state. In the second step, called the measurement update step, the predictions are modified based on the observation equations which compare the predicted observations with the current original ones.

In recent years, many pose estimation algorithms based on the extended Kalman filter (EKF) have been successfully applied to solve the pose estimation problem of a UAS. Tailanian et al. (2014) focus on the sensor fusion of the local sensors of a UAS in which the GNSS and the IMU have been combined by means of an EKF. In Hol et al. (2007), pose estimation on a six DOF robot using an EKF to fuse this information has been shown. Forster et al. (2013) use the collaboration of multiple UAS for pose estimation to combine multiple SLAM algorithms and create an accurate pose estimation. In their approach, real-time camera pose estimation is accomplished by combining the inertial and vision measurements using nonlinear state estimation approaches.

In the case of using nonlinear observation equations, a linearization (realized by means of Taylor series expansion) has to be applied to overcome the nonlinearity issue (see Denham and Pines 1966). In the case where the effects of the linearization errors tend to affect the efficiency of the filter or its convergence, the re-linearization of the measurement equation around the updated state may reduce these difficulties. Therefore, such a procedure is called IEKF. Researchers commonly use explicit observation equations in the IEKF. This means that the observations are considered as a function of the state parameters. Such an observation model is generally called a Gauss Markov model (GMM).

If the equations relating the observations to the state parameters are condition equations, for example, some 3D points should fulfill the plane equations, then we do not exhibit the typical formulation of the GMM. In other words, the observations and the state parameters are not separable (implicit measurement equations, see Dang 2007). In such a case, we are dealing with a Gauss Helmert model (GHM). In Dang (2007, 2008), Steffen and Beder (2007), Steffen (2013) and Ettlinger et al. (2018), an IEKF is used which deals with implicit measurement equations. In Vogel et al. (2018), an IEKF is used for georeferencing and extended with additional nonlinear equality constraints. In Vogel et al. (2019), the approach is further extended by integrating nonlinear inequality constraints. With the ability to handle implicit measurement equations and nonlinear inequality constraints, it is possible to depict almost any mathematical relationship within an IEKF.

### Contribution

In this paper, we present an adaption of the IEKF in Vogel et al. (2018) for *data-driven* georeferencing of a UAS with an accuracy better than 10 cm in position and \(0.1^{\circ}\) in orientation. The highlight of the IEKF presented is the fusion of laser scanner measurements towards building facades, modeled as planes in a 3D city model, with the corresponding plane parameters of the 3D city model. This fusion leads to nonlinear implicit measurement equations and additional nonlinear equality constraints, which are covered within the IEKF. To evaluate the functionality and performance of the IEKF, we developed a simulation tool with a simple dynamic system model defining the state of the UAS and other states. On the basis of the states defined, we simulated sensor measurements regarding the sensor specifications and further characteristics. We tested the IEKF for different scenarios and analyzed its sensitivity towards different data characteristics.

### Outline

The dedicated sections are as follows:

In Sect. 2 we give a detailed description of the UAS, the IEKF implemented and the entire workflow for georeferencing. Section 3 displays the simulation tool developed and gives an overview of the specifically chosen values for the IEKF. We present and discuss the results of the IEKF applied to the simulation in Sect. 4. Finally, conclusions and an outlook are drawn in Sect. 5.

## IEKF for Georeferencing of a UAS

The Geodetic Institute (GIH) and the Institute of Photogrammetry and GeoInformation (IPI) of Leibniz University Hannover (LUH) are currently working on the *UAS project*. The *UAS project* deals with the precise determination of the trajectory of a UAS by integrating camera and laser scanner data in combination with generalized object information. Within this paper, we will focus on the usage of laser scanner data in combination with a non-generalized 3D city model. We will exclusively use simulated data to evaluate the functionality and performance of the algorithms developed. The simulation scenarios were chosen in accordance with common measurement scenarios and data characteristics.

### General Idea

The UAS moves through an urban area where the building facades are modeled as planes within a 3D city model. Three-dimensional city models with a level of detail 1 (block model) and a level of detail 2 (model with differentiated roof structures and boundary surfaces) are freely available for many cities. A detailed classification of the different levels of detail is given in Gröger et al. (2012). The pose of the UAS is roughly known, for example, from measurements of a GNSS receiver and an IMU. A 3D laser scanner captures the environment (red dots in Fig. 1) continuously. These captured 3D points may represent the ground, vegetation, building facades, or other objects. The laser scanner measurements are given in the local sensor coordinate system. The laser scanner measurements are transformed in a superordinate coordinate system according to the roughly known translation \({\varvec{t}}=\left[ t_x,t_y,t_z\right]\) and orientation \({\varvec{o}}=\left[ \omega ,\phi ,\kappa \right]\) (see Fig. 2). The transformed laser scanner measurements are assigned to planes of the 3D city model based on the distance between the scanner points and the planes of the building model (see Sect. 2.3). Only the points that are close enough to a plane of the 3D city model (green points in Fig. 2) are used as observations afterwards. The final pose parameters are estimated within a GHM, resp. filtering approach, by minimizing the distance between the assigned laser scanner measurements and the planes of the 3D city model to which they are assigned.

### UAS

A UAV is equipped with a 3D laser scanner, cameras, a GNSS receiver and an IMU in the *UAS project*. Figure 3 depicts a simplified sketch of the platform setup (without cameras). The laser scanner^{Footnote 5} scans 16 scan lines which are nearly perpendicular to the sensors’ vertical axis (that corresponds to the *z*-axis in Fig. 3). The divergence between each scan line is \(2^{\circ}\). Thus, the laser scanner has a field of view of \(30^{\circ}\times {360}^{\circ}\). It is possible to set the resolution of the points in the scan lines between 0.1° and \(0.4^{\circ}\). The rotation rate depends on that setting and is between 5 Hz and 20 Hz. We set the resolution to \({0.4}^{\circ}\) to obtain a higher rotation rate of 20 Hz. Furthermore, it is possible to exclude certain angle areas from the measurement, because they cannot provide any data or the data generated are not needed for further processing. In our case, it would make sense to exclude the angle areas where the laser scanner measures towards the UAV.

### Assignment Algorithm

A crucial task of our georeferencing process is the assignment of measured 3D points \({\varvec{C}}^{\text {loc}}\), given in a local coordinate system of the laser scanner, to a plane of the city model, given in the global coordinate system. Therefore, we first have to transform \({\varvec{C}}^{\text {loc}}\) to \({\varvec{C}}^{\text {glo}}\):

The parameters of the translation \({\varvec{t}}\) describe the position of the origin of the local coordinate system. The rotation matrix \({\varvec{R}}\) is obtained based on the orientation \({\varvec{o}}=\left[ \omega ,\phi ,\kappa \right]\) of the local coordinate system according to Luhmann (2013):

The assignment is realized by a simple distance criterion, as described in Unger et al. (2016, 2017). The Euclidean distance to each plane of the city model is calculated for each 3D point in \({\varvec{C}}^{\text {glo}}\). Whether the projection of the points into the plane lies within the bounding polygon of the planes is checked for points where the distance to the nearest plane is less than a threshold value \(d_{\text {assign}}\). If it is outside, the distance from the point to the boundary polygon of the plane is calculated and replaces the orthogonal distance. Points that project inside the boundary or are closer than \(d_{\text {assign}}\) are assigned to their closest plane. \(d_{\text {assign}}\) has to be selected regarding the accuracy of the points as well as the accuracy of the city model. The accuracy of the points depends mainly on the accuracy of the translation and orientation parameters \({\varvec{t}}\) and \({\varvec{o}}\) and the accuracy of the measured points \({\varvec{C}}^{\text {loc}}\). The accuracy of the city model depends on the geometrical accuracy of its vertices and the extent of generalization effects by which the model deviates from the reality captured. The larger the threshold is fixed, the more points are assigned to planes, but the higher the probability is that incorrect assignments will be set that lead to incorrect results.

### IEKF (Iterated Extended Kalman Filter)

The IEKF which we adapted for georeferencing of the UAS is given in Vogel et al. (2018). This approach can be applied for many different use cases. All available observations, types of prior information, and requested states need to be linked with the respective uncertainty information. Based on this, all information can be introduced into the IEKF.

According to the respective application, several relationships have to be established by means of linear or rather nonlinear functional models. A system model \({\varvec{f}}(\cdot )\) is needed for the requested states \({\varvec{x}}_\textit{k}\) to describe the physical behavior of the system from epoch \(\textit{k}-1\) to the current epoch \(\textit{k}\). Realization of this transfer is carried out within the prediction step of the IEKF. Suitable functional relationships for each observation \({\varvec{l}}_\textit{k}\) have to be formulated within the measurement model \({{\varvec{h}}}(\cdot )\). Based on implicit (\({{\varvec{h}}}(E({\varvec{l}}),\,{\varvec{x}})=\mathbf 0\)) or explicit (\(E({\varvec{l}})-{{\varvec{h}}}{\varvec{({x})}}=\mathbf 0\)) formulations, relations between the observations available and requested states are considered. \(E(\cdot )\) is the expected operator of a random vector; here, the expected value of the observation vector \({\varvec{l}}\), which can be replaced by \(E({\varvec{l}})={\varvec{l}}+{\varvec{v}}\). This consideration of current observations is carried out within the measurement update step. Available prior information can also be integrated within the measurement model during the measurement update step. In addition, further suitable prior information can also be applied in terms of state constraints by means of additional linear (\({\varvec{D}}{\varvec{x}}_{\textit{k}}={\varvec{d}}\)) or nonlinear (\(\textit{g}\left( {\varvec{x}}_{\textit{k}}\right) ={\varvec{b}}\)) functions. Here, \({\varvec{D}}\) is a known matrix and \({\varvec{d}}\) and \({\varvec{b}}\) are known vectors with respect to related state constraints. Formulation of such restrictions can be implemented by means of equality constraints within the constraint step.

The basic workflow of this IEKF is depicted in Fig. 4 in a simplified way. Detailed equations for initialization, prediction, measurement update, and constraints are given in the following Sects. 2.4.1–2.4.6. Algorithm 1 given in Sect. 2.5 summarizes the required input, different computations steps and the output.

#### State Parameter Vector

The state parameter vector \({\varvec{x}}_{k}\) consists of two parts, as already depicted in Fig. 4. The first part \({\varvec{x}}_{{\text{State}},k}\in {\mathbb {R}}^{9}\) consists of the current position \({\varvec{t}}_{k}\), orientation \({\varvec{o}}_{k}\) and velocity \({\varvec{v}}_{k}\) of the UAS (see Eq. 6). These state parameters describe the state of the UAS.

The second part consists, on one hand, of the vector \({\varvec{x}}_{{\text{Plane}},k}\in {\mathbb {R}}^{4\cdot E}\), which contains the parameters of all the *E* city model planes in Hesse normal form (see Eq. 7). The Hesse normal form is defined by the \(3\times 1\) normal vector \({\varvec{n}}_{e}=[n_{x,e},n_{y,e},n_{z,e}]^{T}\) and the distance to the origin \(d_{e}\) with \(e=1\ldots E\). On the other hand, it consists of the vector \({\varvec{x}}_{V,k}\in {\mathbb {R}}^{3\cdot M}\), which contains all the *M* vertices of the building model (see Eq. 8):

with

The integration of plane parameters and vertices into the state parameter vector is used to identify and correct planes that are not accurately represented in the 3D city model. Although this purpose is not part of this paper, we are already introducing the mathematical relationship.

The complete state parameter vector \({\varvec{x}}_{k}\) is arranged according to:

#### Observation Vector

The MSS mentioned in Sect. 2.2 provides discrete 3D laser scanner point clouds (LSC) and 6D pose information by means of a GNSS receiver and an IMU. An LSC consists of a full scan rotation (\(30^{\circ}\times {360}^{\circ}\)). For the sake of simplification, we will not consider the movement of the UAV during the period of a full scan rotation in the following. This time period is a maximum of 0.05 s for the configuration chosen. It is possible to exclude certain angle areas of the laser scanner, as has already been described in Sect. 2.2. The time period for the non-excluded areas is further reduced by excluding the angular areas in which the laser scanner measures in the direction of the UAV. This is acceptable due to the planned velocity of the UAV of about \(1\,\hbox {m}\,\hbox {s}^{-1}\) and an angular velocity of \(2^{\circ}\,\hbox {s}^{-1}\). In further development steps, the LSC should consist of fewer and temporally closer 3D points, for example, just a half or a quarter scan rotation. In addition, the initial vertices of the 3D city model are introduced as observations into our approach. The observation vector \({\varvec{l}}_{\textit{k}}\) consists of three parts for each epoch \(\textit{k}=1\ldots \textit{K}\). The first part \({\varvec{l}}^{\text {loc}}_{{\text{Scan}},k}\) consists of the measured LSC in the local sensor coordinate system with a total of *N* 3D points. \({\varvec{l}}^{\text {loc}}_{{\text{Scan}},k}\) only contains the 3D points, which were assigned to a plane of the city model^{Footnote 6} (for the assignment algorithm, see Sect. 2.3). These points are assorted on the basis of the planes they are assigned to:

with

and

\({\varvec{l}}^{\text {loc}}_{{\text{Scan}},e,k}\) is representing the vector with all \(N_e\) points assigned to plane *e*. The total number *N* of 3D points of the LSC stored in the observation vector is calculated by:

The second part \({\varvec{l}}^{\text {glo}}_{{\text{Pose}},k}\) consists of the direct GNSS and IMU observations for the position and orientation:

Finally, the third part \({\varvec{l}}^{\text {glo}}_{\textit{V},0}\) consists of the, in total *M*, initial vertices of the *E* model planes^{Footnote 7}:

with

representing a 3D point which is the vertex of at least one plane.

The observation vector is arranged as follows:

We apply Eq. (19) for building the stochastic model of the observation vector. We neglect correlations in all cases for a better discussion of the mainly important issues:

The standard deviation of a laser scanner coordinate component is denoted by \(\sigma _{{\text{LS}}}\). \({\varvec{\sigma }}_{\textit{t}}=\left[ {\sigma }_{\textit{t}},{\sigma }_{\textit{t}},{\sigma }_{\textit{t}}\right]\) and \({\varvec{\sigma }}_{o} = \left[ {\sigma _{o} ,\sigma _{o} ,\sigma _{o} } \right]\) denote the standard deviation of the GNSS and IMU observations, and \(\sigma _V\) denotes the standard deviation of the initial vertices.

#### System Equation

Formulating the system equation of the kind:

we neglected the control vector \({\varvec{u}}\) by setting it to zero. The system noise is normally distributed with \({\varvec{w}}_{k-1}\sim N({\varvec{0}},{\varvec{\Sigma }}_{ww})\). Equation (20) can be formulated as:

where \({\varvec{F}}_{x,k}\) denotes the transition matrix:

Due to the fact that we do not intend to develop an optimal system model within this paper, we applied rather simple linear system equations. Nevertheless, a subsequent adaption of the system model with more complex system equations is easily possible if the future flight characteristics of the UAS require it. For the system equations chosen, the transition matrix \({\varvec{F}}_{x,k}\) is given by:

with

\(\Delta \tau\) is the time period between two consecutive epochs.

We apply the following equations for building the variance–covariance matrix (VCM) of the system noise \({\varvec{\Sigma }}_{ww}\):

The standard deviations of the system noise for translation, orientation, and velocity are denoted by \({\varvec{\sigma }}_{t,w}=\left[ \sigma _{t,w},\sigma _{t,w},\sigma _{t,w}\right]\), \({\varvec{\sigma }}_{o,w}=\left[ \sigma _{o,w},\sigma _{o,w},\sigma _{o,w}\right]\) and \({\varvec{\sigma }}_{v,w}=\left[ \sigma _{v,w},\sigma _{v,w},\sigma _{v,w}\right]\). The system noise for the plane parameters and vertices is given by the standard deviations \({\varvec{\sigma }}_{n,w}=\left[ \sigma _{n,w},\sigma _{n,w},\sigma _{n,w}\right]\), \(\sigma _{d,w}\) and \(\sigma _{V,w}\).

#### Measurement Equation

With the state parameter and observation vector given in Sects. 2.4.1 and 2.4.2, we can formulate three types of nonlinear implicit measurement equations. The first type of measurement equation \({\varvec{h}}_{\mathrm {I}}\) causes that each transformed LSC \({\varvec{l}}^{\text {glo}}_{{\text{Scan}},e,k}\) has to be located in the assigned plane of the city model represented by \({\varvec{n}}_{e}\) and \(d_{e}\). If the point \({\varvec{p}}^{\text {loc}}_{e,i,k}=\left[ X^{\text {loc}}_{e,i,k},Y^{\text {loc}}_{e,i,k},Z^{\text {loc}}_{e,i,k}\right] ^T\) is in \({\varvec{l}}^{\text {loc}}_{{\text{Scan}},e,k}\), which consists of all points assigned to plane *e* of the 3D city model, the following measurement equation is applied:

with \(i\in \{1\ldots N\}\).

The rotation matrix \({\varvec{R}}_k\) is calculated based on the current orientation \({\varvec{o}}_k\) according to Eqs. (2)–(5). Subsequently, we obtain *N* measurement equations of type \({\varvec{h}}_{\mathrm {I}}\):

Figure 5 shows an illustration of the relationship between state parameters, observations, and measurement equations.

In the second type of measurement equation \({\varvec{h}}_{\mathrm {II}}\), we calculate the difference between the estimated pose \(\left[ {\varvec{t}}_{k};{\varvec{o}}_{k}\right]\) and observed pose \({\varvec{l}}^{\text {glo}}_{{\text{Pose}},k}\) of GNSS and IMU:

If the GNSS and IMU data are available, we obtain six measurement equations of type \({\varvec{h}}_{\mathrm {II}}\). When the GNSS signal is missing, the measurement equation changes to:

and we only obtain three measurement equations of this type.

The third type of measurement equation \({\varvec{h}}_{\mathrm {III}}\) is introduced to avoid a datum defect. It computes the difference between the estimated vertices in \({\varvec{x}}_{V,k}\) and the initial vertices in \({\varvec{l}}^{\text {glo}}_{V,0}\). If the vertex \({\varvec{V}}^{\text {glo}}_{m,k}\) is stored in \({\varvec{x}}_{V,k}\) and \({\varvec{V}}^{\text {glo}}_{m,0}\) is its initial observation stored in \({\varvec{l}}^{\text {glo}}_{V,0}\), the following measurement equations are applied:

with \(m\in \{1\ldots M\}\).

For a three-dimensional vertex Eq. (33) consists of three measurement equations. Subsequently, we obtain \(3\cdot M\) measurement equations of type \({\varvec{h}}_{\mathrm {III}}\):

Altogether, the measurement equations are given by:

#### Nonlinear Equality Constraint for the State Parameters

We apply two types of nonlinear equality constraints for the state parameters in our IEKF. The prior information which we want to model describes hard constraints which have to be fulfilled.

The first type of nonlinear equality constraint \({\varvec{g}}_{\mathrm {I}}\) arises due to the fact that we are using plane parameters within the state parameter vector \({\varvec{x}}_{k}\). Here, we must ensure unit normal vectors by means of a length of one. For this, we can make use of the nonlinear equality constraints:

with \(e\in \{1\ldots E\}\).

The nonlinear equality constraint of type \({\varvec{g}}_{\mathrm {I}}\) applies to each of the *E* planes normal vectors:

The right side of the equal sign is stored in vector \({\varvec{b}}_{\mathrm {I}}\):

With the second type of nonlinear equality constraint \({\varvec{g}}_{\mathrm {II}}\), we ensure that each vertex of a plane is located in the plane. By this, we preserve the topology of the 3D city model. If we assume that \({{\varvec{V}}}^{\text {glo}}_{\textit{m,k}}\) is a vertex of plane *e*, represented by \({\varvec{n}}_{e}\) and \(d_{e}\), the following equality constraint is applied:

with \(i\in \{1\ldots 4\cdot E\}\).

This type of nonlinear equality constraint must be fulfilled for each vertex of all planes. In general, 3D city model planes have 4 vertices, but it is also possible for them to have 3 or more than 4 vertices. Thus, the number \(4\cdot E\) is just a rough estimate for the total amount of equality constraints of type \({\varvec{g}}_{\mathrm {II}}\):

The right side of the equal sign is stored in vector \({\varvec{b}}_{\mathrm {II}}\):

A comparable procedure can be found in Unger et al. (2016). Altogether, the nonlinear equality constraints for the state parameters are given by:

#### Initialization

The initial state parameter vector \({\varvec{x}}_{0}\) is created by the GNSS and IMU observations. We assume zero for the velocity in each coordinate component:

The initial plane parameters \({\varvec{n}}_{e,0}\) and \(d_{e,0}\) were estimated from the planes’ vertices stored in the city model by the Drixler algorithm (Drixler 1993). We make use of the vertices of the 3D city model given for the initial vertices \({\varvec{x}}_{V,0}\):

We apply the following equations for building the VCM of the initial state parameter vector \({\varvec{\Sigma }}_{xx,0}\):

The standard deviations of the initial state parameters of translation, orientation, and velocity are denoted by \({\varvec{\sigma }}_{t,0}=\left[ \sigma _{t,0},\sigma _{t,0},\sigma _{t,0}\right]\), \({\varvec{\sigma }}_{o,0}=\left[ \sigma _{o,0},\sigma _{o,0},\sigma _{o,0}\right]\), and \({\varvec{\sigma }}_{v,0}=\left[ \sigma _{v,0},\sigma _{v,0},\sigma _{v,0}\right]\). The standard deviations of the initial plane parameters and vertices are denoted by \({\varvec{\sigma }}_{n,0}=\left[ \sigma _{n,0},\sigma _{n,0},\sigma _{n,0}\right]\), \(\sigma _{d,0}\) and \(\sigma _{V,0}\).

### Workflow

The workflow of our algorithm is summarized in Algorithm 1. This is an adaption of the algorithm proposed in Vogel et al. (2018). We highlighted the cross references to equations and sections in blue. The partial derivatives (see lines 9, 22, 23, and 40) were obtained once using the symbolic Math toolbox of \(\text {Matlab}^{\copyright }\). Subsequently, we implemented the partial derivatives in a function. Another possibility to obtain the partial derivatives could be the use of INTLAB (Rump 1999). We fixed the stop criterion \(c_{\mathrm {stop}}\) to \(1\times 10^{-12}\) in the measurement update step. This stop criterion was reached after an average of six iterations in the subsequent simulation.

## Simulation

Computer simulations are a great tool for analyzing and interpreting engineering systems. Here, we intended to evaluate the functionality and performance of the IEKF implemented. Therefore, we focused on two scenarios which basically conform to the testing environment designated for the *UAS Project*. Furthermore, we focused on the challenging initialization phase of the IEKF and analyzed a rather short trajectory of 2.5 m length and with 50 epochs.

Therefore, we created a model of a fictitious building and a ground plane. Subsequently, we determined a fictitious trajectory beside this building. We assumed a constant velocity for the UAS of 1 m s^{−1} and simulated laser scanner measurements with a frequency of 20 Hz. Consequently, the epochs simulated are at constant distances of 0.05 m. Each of the epochs consists of a laser scan and the desired values of position and orientation obtained from the determined trajectory at a certain time.

Figures 6, 7 and 8 depict different views of the building and ground plane created. The building created contains roof planes (in dark grey), wall planes (in red), and window planes (in transparent blue). Figure 9 depicts the corresponding 3D city model of the building. As we can see, the ground plane is not part of the 3D city model. The 3D city model contains the vertices of each plane and is given in the superordinate coordinate system. The simulated and correctly georeferenced point cloud (blue dots) of the first epoch and the trajectory of the UAS (magenta line) is depicted in each figure.

### Scenarios

We simulated two scenarios in which the laser scanner and IMU measurements were generated differently under certain assumptions regarding measurement accuracy and bias. We repeated the simulation 500 times (\(S=500\)) for both scenarios. In scenario 1, we only added normally distributed noise on the laser scanner measurements, the position (representing the GNSS receiver), and the orientation (representing the IMU). In scenario 2, we systematically perturbed the laser scanner measurements, hitting the windows of the simulated building, and increased the (standard deviation of the) noise for laser scanner measurements, hitting the windows and the ground. The systematic and increased random disturbances of the measurements hitting the windows simulate the infiltration behavior for glass. We included the increased measurement uncertainty and the actual infiltration of the laser into the increased noise of these measurements. The increased noise for measurements hitting the ground simulates possible unevenness and vegetation. Thus, this increased noise is more justified by actual structures in the object space than by an increased uncertainty of the measurement. Furthermore, we added a drift on the \(\kappa\)-component of the IMU. Therefore, we linked the systematic effect \(\Delta\) to the epoch number \(k\in \{1,\ldots ,K\}\). The \(\kappa\)-component conforms to the heading of the UAS. All assumed systematic effects \(\Delta\) and standard deviations \(\sigma\) of the added noise are depicted in Table 1. Basically, the assumptions of scenario 1 should be consistent with the manufacturer’s specifications of the sensors used or planned for our UAS. The assumptions in scenario 2, especially the ones for the laser scanner, are based on experience from test measurements. In our simulation, we assume that the positions and orientations of all sensors in a platform coordinate system or body frame have already been determined in a calibration process. We also assume that the sensors are properly synchronized. To neglect the effect of generalization in the 3D city model, we used the same model for simulation and in the following IEKF algorithm.

### Values Chosen for the IEKF

We used a constant distance threshold \(d_{{{\text{assign}}}} = 0.3\,{\text{m}}\) for the assignment algorithm (see Sect. 2.3) in our simulation. This chosen threshold is a trade-off between the rather imprecise pose in the first epochs and the more precise pose in the last epochs.

The standard deviations used for building the initial VCM \({\varvec{\Sigma }}_{xx,0}\) (see Eqs. 49–52) are depicted at the top of Table 2. The standard deviations of the initial position \(\sigma _{t,0}\) and orientation \(\sigma _{o,0}\) conform to the standard deviations of the GNSS and IMU observations. We assume a standard deviation for the initial velocity \(\sigma _{v,0}\) which includes the planned maximal velocities of the UAV. For the standard deviations of the initial plane parameters \(\sigma _{n,0}\) and \(\sigma _{d,0}\), we met the assumption that these values should be very small. Thus, they correspond roughly to the standard deviation of the initial vertices \(\sigma _{V,0}\), which, in turn, conform to the standard deviation chosen for the observation vector. If the accuracies of the planes’ vertices are known, the standard deviations of the plane parameters could also be determined by means of variance propagation.

The standard deviations chosen for the system noise (see Eqs. 25–28) can be found in the middle of Table 2. We set the standard deviation for translation \(\sigma _{t,w}\), orientation \(\sigma _{o,w}\) and velocity \(\sigma _{v,w}\) depending on the time period \(\Delta \tau\) between two epochs and regarding possible unpredictable movements of the UAS in this time period. We set the system noise to zero for \({\varvec{\Sigma }}_{ww,{\text{Plane}}}\) and \({\varvec{\Sigma }}_{ww,V}\), because these parameters are constant over time. Consequently, \(\sigma _{n,w}\), \(\sigma _{d,w}\), and \(\sigma _{V,w}\) are zero.

We used the simulated standard deviation \(\sigma _{\text{LS}}=0.02\,\hbox {m}\) of the laser scanner measurements for the stochastic model of the observation vector (see Eq. 19). We also introduced the simulated standard deviations \(\sigma _{t}=0.5\,{\text{m}}\) and \(\sigma _{o}=0.2^{\circ}\) for the GNSS and IMU observations. In the case of the initial vertices of the 3D building model, we introduced a small standard deviation of \(\sigma _{V}=0.1\,{\text{mm}}\). The 3D city model is, thus, almost introduced as a fixed feature. We are aware that this assumption is too optimistic and that the accuracy of the introduced 3D city model is rather in the range of a few centimeters. However, in the context of this paper, the influence of a less precise 3D city model will not be considered in more detail.

We assume that \({\varvec{\Sigma }}_{ll,k}\) and \({\varvec{\Sigma }}_{ww}\) do not contain correlations on their own and among themselves.

## Results and Discussion

In the following, we depict the results of the IEKF for both scenarios and compare them with the results of a linear Kalman filter (LKF), which only uses the simulated GNSS and IMU observations. The comparison between LKF and IEKF evaluates the benefit of introducing laser scanner observations and a city model. The state parameter vector \({\varvec{x}}_{k}\) of the LKF solely consists of \({\varvec{x}}_{{\text{State}},k}\) (Eq. 6). The observation vector \({\varvec{l}}_k\) consists of \({\varvec{l}}^{\mathrm {glo}}_{{\text{Pose}},k}\) (Eq. 15). The transition matrix \({\varvec{F}_{x,k}}\) of the LKF is given by \({\varvec{F}}_{x,{\text{State}},k}\) (Eq. 24). Consequently, the VCM of the observation vector and of the system noise have to be adapted. The LKFs’ only remaining measurement equation (Eq. 31) has to be transformed in explicit form. For the LKF, no linearization is necessary and no constraints are used. For more information concerning the LKF, see (Simon 2010). Figures 10, 11, 12, 13, 14 and 15 depict the median values (solid lines) and 68%(\(1\sigma\)) confidence intervals (CI) (dashed lines) for the translation parameters \(t_{x}\), \(t_{y}\), \(t_{z}\) (Figs. 10, 11, 12) and orientation parameters \(\omega\), \(\phi\), \(\kappa\) (Figs. 13, 14, 15) in scenario 1. The CI are calculated numerically using the total number of simulated runs *S* according to Alkhatib et al. (2009). The red lines result from the filtered state parameters \({\hat{{\varvec{x}}}}_{\textit{k}}^{+}\) of the IEKF after the constraint step. The blue lines result from the filtered state parameters \({\hat{{\varvec{x}}}}_{\textit{k}}^{+}\) of the LKF, respectively. The true values are plotted in a dashed black line.

The median of the IEKF result for \(t_{x}\) (across flight direction) in Fig. 10 is very close to the true translation, whereas we can see slight random deviations between the median of the LKF results and the true values. The latter can be seen for each pose component and is not explicitly mentioned again in the following. The CI of the IEKF and the LKF shrink rapidly in the first 5 epochs. Whereas the CI of the IEKF is very close to the median, the CI of the LKF is significantly broader. The latter can be seen for each pose component and is also not explicitly mentioned again. The IEKF performs well for that pose component because of the measurement constellation. Each measurement distinctively assigned to a plane provides information for that pose component. In other words, each measured plane of the 3D city model is sensitive for that pose component.

The results are similar for \(t_{y}\) (in flight direction) in Fig. 11. Again, the median of the IEKF results is very close to the true translation and its CI shrinks within the first epochs. For this pose component, the shrinking of the CI (IEKF) is slightly slower than for \(t_x\) and lasts until epoch 10. A reason for that slight difference can be found in the weaker measurement constellation for that pose component. Only the triangle-shaped protrusions are sensitive for \(t_{y}\). Consequently, there are significantly fewer observations which provide information for that pose component.

Figure 12 shows the results for \(t_{z}\) (up-direction). The median of the IEKF results is very constant, but seems to deviate from the true values by a small systematic shift of about 3 cm. This systematic shift is caused by ground points wrongly assigned to a wall plane of the building. This results in a slight rotation around the *y*-axis (see results for \(\phi\)) in combination with a slight descent of \(t_z\). Again, the CI of the IEKF translation shrinks rapidly within the first 6 epochs. The boundaries of the CI in the first epochs are not symmetrical around the median. They are slightly shifted to larger \(t_z\) values. That means that the IEKF converges slower towards the true translation with initial \(t_z\) values larger than the true translation. An initial \(t_z\) value larger than the true translation may cause significantly fewer assignments in the first epochs. Only the roof planes are sensitive for that pose component.

The results for \(\omega\), which represents the rotation around the *x*-axis, are shown in Fig. 13. After 10 epochs, the median of IEKF and LKF obtain comparable results, whereas the median of the IEKF seems to vary slightly more. The CI of the IEKF shrinks within the first 12 epochs. Similar to the \(t_z\) pose component, there is a shift of the IEKF CI towards larger \(\omega\) values. The only sensitive planes for that pose component are the triangle-shaped protrusions and the roof planes. This may explain the larger variations in the median of the IEKF orientation.

Figures 14, 15 show the results for \(\phi\) (rotation around *y*-axis) and \(\kappa\) (rotation around *z*-axis), respectively. We can see similar characteristics for these pose components. In both cases, the median of the IEKF results is constant but systematically shifted by a small value of about \(0.05^{\circ}\). As has already been mentioned for \(t_{z}\), the systematic shift in \(\phi\) is caused by wrongly assigned ground points. Because of the skewed alignment of the laser scanner, the wrongly assigned ground points also cause a slight rotation in \(\kappa\). All planes are sensitive for both pose components.

For scenario 2, we depicted the results in a similar way in Figs. 16, 17, 18, 19, 20 and 21. In Fig. 16, we can see the results for \(t_{x}\). Similar to scenario 1, the median of the IEKF results comes very close to the true translation. The lower boundary of the CI (IEKF) is farther afield from the median than the upper boundary. It shrinks until epoch 14. An explanation for the one-sided enlarged CI (IEKF) is the systematically extended laser scanner measurements hitting the windows. With an initial \(t_x\) translation, which is farther afield from the building than the true translation, these systematically extended laser scanner measurements are wrongly assigned to the walls of the building. After some epochs, these false assignments seem to have disappeared. These effects are analyzed later on (see Fig. 29).

The median of the IEKF results for \(t_y\) in Fig. 17 is close to the true translation. The upper boundary of the CI (IEKF) shrinks until epoch 13, whereas the lower boundary of the CI (IEKF) clearly departs from the median from epoch 14 on. Again, these effects are analyzed later on (see Figs. 28 and 29).

We can see the results for \(t_z\) in scenario 2 in Fig. 18. The results are comparable to the ones obtained in scenario 1 for that pose component. A significant difference can be found in the upper boundary of the CI (IEKF), which converges to the median much slower than in scenario 1. In scenario 2, it does not converge until epoch 15. Again, initial \(t_z\) values larger than the true position cause significantly fewer assignments in the first epochs.

We observe comparable results to scenario 1 for the pose parameters \(\omega\) and \(\phi\) in Figs. 19 and 20. In both figures, we can observe that one boundary of the CI (IEKF) is farther afield from the median than the other and that the CI (IEKF) shrinks slower than in scenario 1.

The pose component \(\kappa\) is depicted in Fig. 21. The CI (IEKF) shrinks until epoch 12. In addition, we can see that the simulated drift in \(\kappa\) does not affect the IEKF results, whereas the median of the LKF departs from the true orientation continuously as simulated.

A suitable value to evaluate the performances of the filters is the root-mean-square error (RMSE). In our simulation, the RMSE is the error between filtered state parameters and the true state parameters. The RMSE of the pose parameter \(t_x\) for run \(s\in \{1,\ldots ,S\}\) is calculated according to:

\(t_{x,s,k}\) results from the filtered state parameter vector \({\hat{\mathbf{x }}}_k^{+}\) from the IEKF or the LKF, respectively. \({\bar{t}}_{x,k}\) are the true values. The calculation of the RMSE for the other pose parameters is carried out analogously.

Table 3 depicts the characteristic criterions: Minimum, maximum, mean, median, standard deviation (SD), and the lower bound (\(\downarrow\)) and upper bound (\(\uparrow\)) of the 68% and 95% CI of different RMSE. In the last row of Table 3, we display the rate of runs where the IEKF obtained a smaller RMSE than LKF and vice versa. Each criterion is divided into two rows regarding scenario 1 (above) and scenario 2 (below).

Table 3 clearly shows that the IEKF obtains better results than the LKF. The median values especially are significantly smaller in each state parameter and each scenario. By contrast, the range of the IEKF results is significantly larger than the range of the LKF results, which can be seen in the SD and 95% CI values. A possible explanation can be found in a larger number of runs, where the IEKF is far away from the true values. We will subsequently call these runs *failures*. These *failures* can be caused by a disadvantageous initial pose parameter in the first epoch. By this, a large number of laser scanner measurements are assigned to wrong planes and the IEKF *converges* to a wrong pose. The interaction between initial pose parameters and performance of the IEKF is analyzed at the end of this section. The *failures* are also the explanation for the large discrepancy between the mean and median values.

To evaluate the performance of the IEKF over the epochs, we calculated a mean RMSE for each epoch over all *S* simulation runs. The calculation is as follows for the RMSE of \(t_x\) in epoch \(k\in \{1,\ldots ,K\}\):

We obtain the RMSE for the other pose parameters analogously. The results can be found in Figs. 22, 23, 24, 25, 26 and 27.

Figures 22, 23, 24, 25, 26 and 27 generally support the interpretations made before. The RMSE of the translation parameters \(t_{x}\) and \(t_{z}\) determined by the IEKF decrease with an increasing number of epochs. At the last epoch, the \({\text{RMSE}}_{t_x,{\text{Ep}}.}\) and \({\text{RMSE}}_{t_z,{\text{Ep}}.}\) of the IEKF is significantly smaller than the RMSE obtained by LKF in both scenarios. Again, \({\text{RMSE}}_{t_y,{\text{Ep}}.}\) differs and increases after epoch 10 with an increasing number of epochs. In both scenarios, especially in scenario 2, the \({\text{RMSE}}_{t_y,{\text{Ep}}.}\) obtained by the IEKF is larger than the one obtained by LKF in the last epochs. The RMSE of the orientation parameter show different characteristics, but they are intrinsically quite similar. The RMSE obtained by IEKF starts with large values and significantly decreases after epoch 10. In the last epoch, \({\text{RMSE}}_{\phi ,{\text{Ep}}.}\) and \({\text{RMSE}}_{\kappa ,{\text{Ep}}.}\) of IEKF and LKF are quite similar. \({\text{RMSE}}_{\omega ,{\text{Ep}}.}\) of the IEKF undercuts the \({\text{RMSE}}_{\omega ,{\text{Ep}}.}\) of the LKF until epoch 9 (scenario 1), resp. epoch 30 (scenario 2). The \({\text{RMSE}}_{\kappa ,{\text{Ep}}.}\) of LKF in scenario 2 is clearly affected by the simulated IMU drift. It is remarkable that (except in \({\text{RMSE}}_{t_y,{\text{Ep}}.}\)) the RMSE of the IEKF between scenario 1 and scenario 2 is very similar in the last epoch.

For a deeper understanding of the link between initialization and performance of the IEKF, we plotted the initial values of GNSS and IMU of the first epoch in parallel coordinates. The parallel coordinate representation was first used by Inselberg (1985). For this representation, we will show all pose components in six parallel axes. On each axis, all initial values belonging to this component are depicted. This representation is used to identify whether some extreme initial pose values could have had a large impact on the divergence of the IEKF. In addition, we colored the runs where the IEKF failed (*failure*) in orange and the others (*success*) in blue. The distinction in *failure* and *success* was selected based on the RMSE of the position in the last epoch. The run is classified as a *failure* when there is a value larger than 10 cm.

In scenario 1 (Fig. 28), the failure rate is 7.6%. For the initial pose parameters \(t_z\), \(\omega\), \(\phi\), and \(\kappa\), the *failures* are distributed over the whole value range. For \(t_x\), the *failures* are slightly concentrated in a range between − 9.2 and − 8.55 m and a second range around − 10.6 m. For \(t_y\), the *failures* are even more concentrated in a range between 3.72 and 4.3 m. Initial values for \(t_y\) in a range between 3.72 and 4.3 m lead to most of the *failures*. As previously mentioned, we have a weak measurement constellation for \(t_y\). Furthermore, the trajectory is parallel to the *y*-axis with a true value of \(t_y = 7.5\,{\text{m}}\) in epoch 50, the consequence being that with initial values for \(t_y\) between the true value (5 m) and 6.19 m, the IEKF may get back to the true values in a later epoch. This possibility is not given for initial values for \(t_y\) between 3.72 and 4.3 m.

In scenario 2, the failure rate of 20.4% is significantly higher. Again, for the initial pose parameters \(t_z\), \(\omega\), \(\phi\), and \(\kappa\), the *failures* are distributed over the whole value range. For \(t_y\), the *failures* are mainly in a range between 3.72 and 5 m. The reason for that has already been explained for scenario 1. For \(t_x\), we can see a strong concentration of *failures* in a range between − 11.37 and − 10.4 m. Thus, especially that range for \(t_x\) causes the *failures* in scenario 2. The reason for that can be found in the systematically perturbed laser scanner measurements hitting the windows. These measurements are extended by an additive value \(\Delta = 0.6\,{\text{m}}\). In combination with a disadvantageous initialization, these perturbed measurements are wrongly assigned to the wall plane and, therefore, cause the *failures*.

We used GNSS and IMU observations in the measurement update step for all the displayed results in this section. We performed the same experiment without using the GNSS and IMU observations in the measurement update step. The differences in the results are insignificantly small. Therefore, we do not show these results here.

## Conclusions and Outlook

In this paper, we presented the mathematical basics and the workflow of an IEKF which makes it possible to determine the trajectory of a UAS better than 5 cm in position (median value) and \(0.08^{\circ}\) for orientation (median value). It is not mandatory to have continuous GNSS and IMU observations for the implemented IEKF. The trajectory is mainly obtained using laser scanner measurements of building facades, which are modeled as planes in a 3D city model. The laser scanner measurements and the planes of the 3D city model are combined by implicit measurement equations and nonlinear equality constraints within the IEKF.

To demonstrate the functionality and performance of the IEKF implemented, we developed a simulator, which showed that the IEKF is even suited to handle systematically perturbed observations.

Nevertheless, the algorithm demonstrated may be tuned to deal with disadvantageous initial values. A possible starting point can be found in the threshold of the assignment algorithm. Instead of a constant value, this value should be varied regarding the estimated accuracy of the predicted position.

In the future, we plan to evaluate the performance of the IEKF based on real data, especially regarding effects of generalization of the 3D city model and effects of a 3D city model with larger uncertainties. Thus, we can also check whether the assumptions made for the simulation (see Table 1) are also valid in reality. In particular, the performance of the GNSS receiver can deviate strongly from the simulated data with the same precision.

In the IEKF presented, the plane parameters and vertices are introduced as parameters to preserve the topology of the 3D city model. With an increasing number of planes and vertices, the computation becomes inefficient. Here, we plan to reorganize the IEKF presented by means of a dual-state Kalman filter.

As has already been mentioned, we plan to integrate camera measurements into the IEKF. This enables the stabilization of the trajectory in the long term and the improvement of the IEKF’s initial behavior. Therefore, we have to integrate the collinearity equations into our approach. Subsequently, we have to integrate the object coordinates of the tie points into the state parameter vector. This emphasizes the need to use a dual-state Kalman filter. By integrating the camera measurements^{Footnote 8} into the IEKF, we must deal with observations captured in different epochs: one or more images observing a tie point were taken in a past epoch and only one image is taken in the current epoch.

Finally, we aim to analyze the benefit of integrating further geometries in addition to the planes of the 3D city model. There are many cylindrical-shaped objects in urban areas, such as lantern, traffic lights or street signs, which may be used in the IEKF.

## Notes

Also called the world coordinate system.

E.g. the GNSS-IMU system in SBG Systems (2019).

Also called

*sensor-driven*.Also called

*target-based*.Conforms to the Velodyne LiDAR PUCK VLP-16.

In photogrammetry, tie points are observed in more than one image and, therefore, connect poses. By contrast, points measured by the laser scanner are only observed from one pose and do not, therefore, contribute to pose estimation in this scenario as long as they are not related to a model plane.

Please note the missing

*k*in the index, which implies that this observation remains unchanged over the epochs.We do not plan to use stereo cameras.

## References

Abmayr T, Härtl F, Hirzinger G, Burschka D, Fröhlich C (2008) A correlation based target finder for terrestrial laser scanning. J Appl Geod 2(3):429. https://doi.org/10.1515/JAG.2008.015

Alkhatib H, Neumann I, Kutterer H (2009) Uncertainty modeling of random and systematic errors by means of Monte Carlo and fuzzy techniques. J Appl Geod 3(2):136. https://doi.org/10.1515/JAG.2009.008

Besl PJ, McKay ND (1992) A method for registration of 3-D shapes. IEEE Trans Pattern Anal Mach Intell 14(2):239–256. https://doi.org/10.1109/34.121791

Dang T (2007) Kontinuierliche Selbstkalibrierung von Stereokameras. Schriftenreihe/Institut für Mess- und Regelungstechnik, Karlsruher Institut für Technologie vol Nr. 008. Univ.-Verl. Karlsruhe, Karlsruhe

Dang T (2008) An iterative parameter estimation method for observation models with nonlinear constraints. Metrol Meas Syst 15(4):421–432

Denham WF, Pines S (1966) Sequential estimation when measurement function nonlinearity is comparable to measurement error. AIAA J 4(6):1071–1076. https://doi.org/10.2514/3.3606

Dennig D, Bureick J, Link J, Diener D, Hesse C, Neumann I (2017) Comprehensive and highly accurate measurements of crane runways, profiles and fastenings. Sensors (Basel, Switzerland). https://doi.org/10.3390/s17051118

Drixler E (1993) Analyse der Form und Lage von Objekten im Raum. Deutsche Geodätische Kommission. Reihe C. Dissertationen vol Heft Nr. 409. Beck, München

Elkhrachy I, Niemeier W (2006) Stochastic assessment of terrestrial laser scanner. In: Proceedings of the ASPRS annual conference, Reno, United States of America

Ettlinger A, Neuner H, Burgess T (2018) Development of a Kalman filter in the Gauss-Helmert model for reliability analysis in orientation determination with smartphone sensors. Sensors (Basel, Switzerland). https://doi.org/10.3390/s18020414

Forster C, Lynen S, Kneip L, Scaramuzza D (2013) Collaborative monocular slam with multiple micro aerial vehicles. In: 2013 IEEE/RSJ International conference on intelligent robots and systems (IROS), pp 3962–3970. IEEE, Piscataway, NJ. https://doi.org/10.1109/IROS.2013.6696923

Glira P, Pfeifer N, Briese C, Ressl C (2015) Rigorous strip adjustment of airborne laserscanning data based on the ICP algorithm. ISPRS Ann Photogramm Remote Sens Spat Inf Sci 2:73–80. https://doi.org/10.5194/isprsannals-II-3-W5-73-2015

Gröger G, Kolbe TH, Nagel C, Häfele KH (2012) OGC City Geography Markup Language (CityGML) encoding standard, 2.0.0 edn. Open Geospatial Consortium, Wayland

Hartmann J, Trusheim P, Alkhatib H, Paffenholz JA, Diener D, Neumann I (2018) High accurate pointwise (geo-)referencing of a k-tls based multi-sensor-system. ISPRS Ann Photogramm Remote Sens Spat Inf Sci 4:81–88. https://doi.org/10.5194/isprs-annals-IV-4-81-2018

Hebel M, Arens M, Stilla U (2009) Utilization of 3D city models and airborne laser scanning for terrain-based navigation of helicopters and UAVS. Int Arch Photogramm Remote Sens Spat Inf Sci 38-3/W4:187–192

Hol JD, Schön TB, Luinge H, Slycke PJ, Gustafsson F (2007) Robust real-time tracking by fusing measurements from inertial and vision sensors. J Real Time Image Process 2(2):149–160. https://doi.org/10.1007/s11554-007-0040-2

Holst C, Kuhlmann H, Paffenholz JA, Neumann I (2015) Tls im statischen, stop & go sowie kinematischen einsatz. In: Terrestrisches Laserscanning 2015 (TLS 2015), Beiträge zum ... DVW-Seminar, vol 147. Wißner-Verlag

**[Erscheinungsort nicht ermittelbar]**Inselberg A (1985) The plane with parallel coordinates. Vis Comput 1(2):69–91. https://doi.org/10.1007/BF01898350

Jutzi B, Weinmann M, Meidow J (2013) Improved uav-borne 3D mapping by fusing optical and laserscanner data. ISPRS Int Arch Photogramm Remote Sens Spat Inf Sci 2:223–228. https://doi.org/10.5194/isprsarchives-XL-1-W2-223-2013

Kaul L, Zlot R, Bosse M (2015) Continuous-time three-dimensional mapping for micro aerial vehicles with a passively actuated rotating laser scanner. J Field Robot. https://doi.org/10.1002/rob.21614

Lee KW, Wijesoma S, Guzmán JI (2007) A constrained SLAM approach to robust and accurate localisation of autonomous ground vehicles. Robot Auton Syst 55(7):527–540. https://doi.org/10.1016/j.robot.2007.02.004

Li-Chee-Ming J, Armenakis C (2013) Determination of UAS trajectory in a known environment from FPV video. Int Arch Photogramm Remote Sens Spat Inf Sci 40-1/W2:247–252

Luhmann T (2013) Close range photogrammetry: 3D imaging techniques. De Gruyter, Berlin

Nguyen V, Harati A, Martinelli A, Siegwart R, Tomatis N (2006) Orthogonal SLAM: a step toward lightweight indoor autonomous navigation. In: 2006 IEEE/RSJ International conference on intelligent robots and systems, pp 5007–5012. https://doi.org/10.1109/IROS.2006.282527

Nüchter A, Borrmann D, Koch P, Kühn M, May S (2015) A man-portable, IMU-free mobile mapping system. ISPRS Ann Photogramm Remote Sens Spat Inf Sci 3:17–23. https://doi.org/10.5194/isprsannals-II-3-W5-17-2015

Paffenholz JA (2012) Direct geo-referencing of 3D point clouds with 3D positioning sensors. In: Zugl.: Hannover, Univ., Diss., 2012, Wissenschaftliche Arbeiten der Fachrichtung Geodäsie und Geoinformatik der Leibniz Universität Hannover, vol 302. Univ, Hannover

Rump S (1999) INTLAB—INTerval LABoratory. In: Csendes T (ed) Developments in reliable computing. Kluwer Academic Publishers, Dordrecht, pp 77–104. http://www.ti3.tuhh.de/rump/

SBG Systems (2019) Miniature high performance inertial sensors: Elipse 2 series. https://www.sbg-systems.com/wp-content/uploads/Ellipse_Series_Leaflet.pdf. Accessed 14 May 2019

Schuhmacher S, Böhm J (2005) Georeferencing of terrestrial laserscanner data for applications in architectural modeling. https://doi.org/10.18419/opus-3749

Simon D (2010) Kalman filtering with state constraints: a survey of linear and nonlinear algorithms. IET Control Theory Appl 4(8):1303–1318. https://doi.org/10.1049/iet-cta.2009.0032

Soloviev A, Bates D, van Graas F (2007) Tight coupling of laser scanner and inertial measurements for a fully autonomous relative navigation solution. Navigation 54(3):189–205

Steffen R (2013) A robust iterative Kalman filter based on implicit measurement equations. Photogramm Fernerkundung Geoinf 2013(4):323–332. https://doi.org/10.1127/1432-8364/2013/0180

Steffen R, Beder C (2007) Recursive estimation with implicit constraints. In: Hamprecht FA, Schnörr C, Jähne B (eds) Pattern recognition. Lecture notes in computer science, vol 4713. Springer, Berlin, pp 194–203

Tailanian M, Paternain S, Rosa R, Canetti R (2014) Design and implementation of sensor data fusion for an autonomous quadrotor. In: 2014 IEEE International instrumentation and measurement technology conference (I2MTC 2014). IEEE, Piscataway, NJ, pp 1431–1436. https://doi.org/10.1109/I2MTC.2014.6860982

Talaya J, Alamus R, Bosch E, Serra A, Kornus W, Baron A (2004) Integration of a terrestrial laser scanner with GPS/IMU orientation sensors. In: Proceedings of the XXth ISPRS congress, Istanbul, Turkey, vol 1223

Unger J, Rottensteiner F, Heipke C (2016) Integration of a generalised building model into the pose estimation of UAS images. Int Arch Photogramm Remote Sens Spat Inf Sci 89:1057–1064. https://doi.org/10.5194/isprsarchives-XLI-B1-1057-2016

Unger J, Rottensteiner F, Heipke C (2017) Assigning tie points to a generalised building model for UAS image orientation. ISPRS Int Arch Photogramm Remote Sens Spat Inf Sci 2:385–392. https://doi.org/10.5194/isprs-archives-XLII-2-W6-385-2017

Vogel S, Alkhatib H, Bureick J, Moftizadeh R, Neumann I (2019) Georeferencing of laser scanner-based kinematic multi-sensor systems in the context of iterated extended Kalman filters using geometrical constraints. Sensors (Basel, Switzerland). https://doi.org/10.3390/s19102280

Vogel S, Alkhatib H, Neumann I (2016) Accurate indoor georeferencing with kinematic multi sensor systems. In: 2016 International conference on indoor positioning and indoor navigation (IPIN), pp 1–8. https://doi.org/10.1109/IPIN.2016.7743601

Vogel S, Alkhatib H, Neumann I (2018) Iterated extended kalman filter with implicit measurement equation and nonlinear constraints for information-based georeferencing. In: 2018 International conference on information fusion (FUSION), pp 1209–1216. https://doi.org/10.23919/ICIF.2018.8455258

## Acknowledgements

This work was funded by the Deutsche Forschungsgemeinschaft (DFG, German Research Foundation)—NE 1453/5-1, HE 1822/37-1, and as part of the Research Training Group i.c.sens [RTG 2159]. The computations were performed by the compute cluster, which is funded by the Leibniz University Hannover, the Lower Saxony Ministry of Science and Culture (MWK), and DFG.

## Author information

### Authors and Affiliations

### Corresponding author

## Rights and permissions

**Open Access** This article is distributed under the terms of the Creative Commons Attribution 4.0 International License (http://creativecommons.org/licenses/by/4.0/), which permits unrestricted use, distribution, and reproduction in any medium, provided you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons license, and indicate if changes were made.

## About this article

### Cite this article

Bureick, J., Vogel, S., Neumann, I. *et al.* Georeferencing of an Unmanned Aerial System by Means of an Iterated Extended Kalman Filter Using a 3D City Model.
*PFG* **87**, 229–247 (2019). https://doi.org/10.1007/s41064-019-00084-x

Received:

Accepted:

Published:

Issue Date:

DOI: https://doi.org/10.1007/s41064-019-00084-x

### Keywords

- Iterated extended Kalman filter
- 3D city model
- Unmanned aerial system
- Laser scanner measurements
- Implicit measurement equation
- Equality constraint