High Precision Positioning with Multi-Camera Setups: Adaptive Kalman Fusion Algorithm for Fiducial Markers.

The paper addresses the problem of fusing the measurements from multiple cameras in order to estimate the position of fiducial markers. The objectives are to increase the precision and to extend the working area of the system. The proposed fusion method employs an adaptive Kalman algorithm which is used for calibrating the setup of cameras as well as for estimating the pose of the marker. Special measures are taken in order to mitigate the effect of the measurement noise. The proposed method is further tested in different scenarios using a Monte Carlo simulation, whose qualitative precision results are determined and compared. The solution is designed for specific positioning and alignment tasks in physics experiments, but also, has a degree of generality that makes it suitable for a wider range of applications.

1. Introduction

The evolution of technology has led to an increasing demand for solving complex problems which may be viewed as attempts to control and direct system behaviours towards desired states. The inherent complexity of problems and processes requires new approaches both in system modelling and in defining the emergent interaction with a highly dynamical and sparsely defined environment.

Cognitive approaches are successfully used in contexts where the boundary between the systems and the environment is fuzzy. However, they exhibit strong interrelation and interconnection, assisted by specific perception mechanisms [1]. Advances in complex control applications can be achieved only by considering adequate design approaches for sensory systems, especially in domains like environmental applications [2], health [3], industrial control, agriculture, etc.

Although new technologies such as wireless sensor networks or Internet of Things (IoT) are providing valuable solutions for appropriate perception mechanisms, complex issues are raised with the inclusion of data fusion, reliability, flexibility, reconfigurability, and cost of the measurement system. If some of these problems can be addressed during the modelling process, there are others (e.g., sensor positioning and sensor dynamics) that have a bigger impact on the generality of the overall solution.

These issues are specific in critical infrastructures, research institutes, power systems, e-health, mining, and traffic control, where the multitude of concurrent dynamics generating a large amount of information requires adequate solving methods that cover aspects such as relevance, efficiency, and cost optimisation.

This paper follows the development of a measurement method used in a very precise and yet flexible and portable positioning system. Firstly, the method is an answer to a specific application: Automatic alignment with high precision of various instruments which have to be remotely operated in highly flexible, open, and dynamic configurations for physics experiments. However, it can be used in any application where high precision positioning over a large working area is required, where no absolute reference can be defined, or when it is necessary to simultaneously align multiple features in relative positions, with high reliability. Such applications include large scale construction sites where multiple components need to be positioned in precise locations, manufacturing facilities where large machines are assembled or spacecraft docking in zero gravity environments [4].

The article has the following structure. The specific application and context addressed in this work are presented in Section 2. The theoretical context behind the proposed algorithm is described in Section 3. It it then used to build two essential procedures for the positioning system. The precision of the proposed method is assessed in the Section 4 using a Monte Carlo simulation. The conclusions and future developments are detailed in Section 5.

2. Description of the Initial Problem

The large number of high repetition rate, ultrashort pulse, and high power laser facilities that will come online all around the world will require state-of-the-art tools to allow the harnessing of their full potential [5]. The high power lasers of the ELI-NP (Extreme Light Infrastructure-Nuclear Physics) user facility will be employed for a wide range of research topics like studies of nuclear induced reactions, dark matter search, material irradiation, or medical applications [6]. The development of the experimental setups for studying these topics requires specific instrumentation, while also having strict needs in terms of positioning and alignment, in order to ensure optimal experimental conditions.

As the setups are continuously changing, absolute position referencing is hard to achieve. This is a necessity, as multiple instruments need to be precisely positioned relative to each other during the experiment. Figure 1 displays an example of a setup for solid target alignment in which multiple optical diagnostics are positioned using motorised manipulators that have 3 to 6 degrees of freedom (DOF).

Figure 1

Solid target alignment setup example where multiple instruments are positioned using motorised manipulators.

Apart from the precision requirements, additional ones should be taken into consideration. The alignment should be done remotely because the experiments take place inside vacuum chambers and behind concrete walls used for radiation shielding. Moreover, to take advantage of the high repetition rate of the lasers and to maximise the beam-time available for the users, the positioning has to be performed in a limited amount time. In a previous work [7], we addressed this series of requirements and constraints by developing an automatic alignment system that is based on relative position measurements using imaging cameras and compact and flat fiducial markers, due to space and visibility constraints. The alignment algorithm was based on a real-time optimisation procedure which is the subject of a patent [8].

Although the fiducial markers were initially developed and used in augmented reality applications [9], due to their versatility in determining their position in a non-invasive manner (by imaging them with a camera), they were quickly adopted for a different range of applications. These include kinematic calibration and visual servoing for industrial robots [10,11], robot localisation and navigation [12,13,14], SLAM (simultaneous localisation and mapping) [15,16,17] and sensor fusion [18,19,20].

The motivation behind this work is to combine the measurements from multiple cameras in order to increase the working area of the system and to maximise the positioning precision. Simultaneously, the main focus is to present and test the precision of the proposed methods and algorithm, comparing the results with those recorded while using only one camera [21].

3. Method

The basic approach behind the proposed method is to use multiple cameras in order to improve the precision of estimating the pose of fiducial markers and to extend the working area of the system. It begins with the assumption that each camera provides measurements that are erroneous and noisy. The problem can be conceived through an analogy with the GPS system where distance measurements from multiple satellites are used to estimate the position of the receiver module with high precision. The key ingredient in the GPS system is to know very precisely the position of the satellites. The proposed positioning system needs to meet the same requirement for its cameras, but wasting too much effort on this task diminishes its practicality. Consequently it is only assumed that the cameras have unknown but fixed positions.

A related approach that has similar objectives can be found in [22]. The main benefit of the method is the possibility to calibrate the setup of cameras using a 3D feature with fiducial markers having unknown configuration. However, it is not meant for estimating the pose of single markers. In order to achieve this, additional methods are required.

Our method is developed using the ArUco fiducial markers [23,24]. The ArUco library detects and estimates the pose of the marker with respect to the camera using the solvepnp algorithm in which the pinhole camera model is data fitted using a Levenberg–Marquardt non-linear optimisation procedure. The output is represented by the extrinsic camera parameters which can be expressed in terms of a homogenous transformation (the transformation between the camera attached reference frame and the marker attached one) detailed in [7].

3.1. Adaptive Kalman Fusion Algorithm for Multiple Cameras and Fiducial Markers

The method consists of fusing noisy measurements from multiple cameras, in order to improve the precision of estimating the exact position of the fiducial marker. In the scientific literature, multiple sensor fusion and noise filtering methods have been developed over the years. Among them there is the Kalman filter [25], which is the most widely used. The noise effect is mitigated by using a dynamical model for the physical process involved and a statistical model (covariance matrix) for the noise in the measurement process.

For a discrete linear state-space dynamical model in Equation (1), the Kalman filter estimates the value of the state vector x using noisy measurements for the input u and the output y.

In standard data fusion applications that use the Kalman filter, the pose of various objects is estimated using the input from accelerometer, gyroscope, and magnetometer sensors [26,27] and the output from distance measurement devices [28,29]. Our application makes impossible to use any type of attached sensors and, hence, we only rely on the “non-invasive” pose measurement using the solvepnp algorithm and imaging cameras.

Our approach is to employ a state-space model with free dynamics (where u is zero) and with an identity state matrix (A). In this respect, each measurement is made using only one snapshot image from all the cameras that are synchronised with the help of an electrical trigger signal. In this way, the position of the fiducial marker during the measurement is considered fixed. On the same set of images, the solvepnp algorithm is applied multiple times and, thus, the evolution of the measurement is caused only by the noise and not by the movement of the fiducial marker. Furthermore, the measurements are used to iteratively estimate the real position using the proposed algorithm.

In order to improve the results, we propose a procedure designed to update the statistical model of the noise. The Q and R covariance matrices (which are discussed below) are continuously adjusted using the newly acquired data. Any change of the noise behaviour and any existing correlations are captured and thus, the effect of the noise is mitigated more efficiently. Consequently, the proposed algorithm is an adaptive Kalman version.

The homogeneous transformation representation () has numerous practical benefits, especially for pose composition and inversion operations, but it is not suitable in this circumstance because it is redundant (16 numerical values for expressing a 6 DOF pose). The solution is to use an equivalent representation which is composed of the set of translation coordinates (X, Y, and Z) and the set of Euler angles (, , and ).

In this particular case, the structure of the state vector is the real pose of the fiducial marker defined in Equation (2), where the subscript k denotes the present discrete-time sample of the state vector.

The output vector is composed of the pose elements measured using all the cameras available. The structure of the output vector is defined in Equation (3), where the superscript i is indicating the index of the camera considered.

The discrete-time state-space model considered is defined in Equation (4), where is the identity matrix of rank 6, is a random noise signal with normal distribution (white noise) that is quantifying the false evolution induced by the noisy measurement, the H is the measurement model matrix defined in Equation (5), and is the measurement noise also considered white noise.

The equations that describe the Kalman filter are presented in Equation (6). The first two equations give a rough state estimate using the dynamical model while the last 5 equations are used for improving the estimate using the newly acquired output sample. P is a covariance matrix that expresses the confidence degree of the state estimation, which is updated during the algorithm iterations, Q and R are the covariance matrices associated with the noises q and r respectively, is the rough estimation error (difference between the measured output and the predicted one using the first two equations), is the covariance matrix associated with the predicted output, and is the Kalman state update.

The filter requires good estimates for the initial state and the covariance matrices Q and R. In the proposed algorithm this is achieved using an initialisation procedure. The position of the fiducial marker is measured for number of sampling times. The result is the set of samples defined in Equation (3) for (the length of the initialisation) and (the number of cameras available).

Averaging the samples gives a good estimate for the initial state, which is built according to Equation (7), where is the mean operator (expected value). The Q and R matrices are computed from the same set of samples, assuming that there is no correlation between the noises affecting the measurements.

Q is a diagonal matrix defined in Equation (8) built using the mean variance of the pose elements (X, Y, Z, , , and ) along all the cameras. The R matrix is defined in Equation (9) where is defined in Equation (10), built using the variance for each pose element measured by each camera.

After the initialisation is finished, the Kalman algorithm is iterated for number of sampling times, while at each step, a new set of samples in the form of Equation (3) is measured and an estimated state trajectory is built ( for ).

Newly measured system outputs can be used to improve the statistical models of the noises for increased performance. Thereby, at each iteration, every new set is added to the initialisation set and the covariance matrices Q and R are updated using Equations (11)–(13) for .

Considering that the estimated state trajectory () belongs to a system with free dynamics where, in the absence of the noise effects, the state should be constant, a final estimate with a better precision can be achieved by averaging the values of the estimated state trajectory according to Equation (14). The timeline of all the procedures that are involved in the proposed algorithm is presented in Figure 2. In Algorithm 1 the proposed procedure is summarised.

Figure 2

The sequence of procedures involved in the proposed algorithm.

3.2. Setup Calibration Procedure

Given a set of n cameras in a pre-defined fixed configuration, having unknown absolute positions, the purpose of the setup calibration procedure is to determine, in a precise manner, their relative positions. As depicted in Figure 3, the goal is to determine the set of homogenous transformations where and .

Figure 3

The calibration of a set of n cameras requires to determine their relative positions .

This requires the use of a precision gauge, which can be manufactured in the form of a cube of fiducial markers like in [21] or in any 3D shape where the markers can be viewed all around. The gauge needs to be manufactured or calibrated with increased precision. It can be seen as the absolute precision reference used to calibrate the entire positioning system. In Figure 4, a conceptual diagram of a gauge containing m fiducial markers is presented. The set of homogenous transformations between all the markers ( where and ) is precisely known.

Figure 4

A setup of cameras is calibrated using a precision gauge. It is a manufactured feature having m fiducial markers where all their relative positions are precisely known.

The camera setup calibration procedure is done simultaneously for all the camera pairs , where and , with the goal of estimating the homogenous transformation (as presented in Figure 5). It starts with placing the precision gauge inside the environment. Depending on the orientation, each camera sees a different portion of the gauge, i.e., from all n fiducial markers, can measure the position of markers and can measure only , where and . It is preferable that at least one fiducial marker is seen by both cameras, otherwise, it can be virtually determined using the gauge transformations.

Figure 5

The process of calibrating all the pairs of n cameras, which illustrates all the homogenous transformations involved in finding the relative position of and cameras.

The proposed algorithm estimates the homogenous transformation using the noisy measurements from all markers using the camera. This transformation is expressed multiple times in terms of pose measurement (where ) using Equation (15). The transformation is precisely known from the gauge.

The resulting pose is converted to translations and Euler angles which are used for building the output measurement vector of Equation (3).

The proposed algorithm is further applied and the result is an estimation of having a higher degree of precision and being closer to the real value. For the camera, is estimated in a similar manner. Consequently, transformation is computed using Equation (16).

3.3. Position Estimation Procedure

Having a calibrated camera setup, the aim of this procedure is to estimate the position of a fiducial marker attached to a specific instrument, according to the application.

In Figure 6, the conceptual diagram of this procedure is depicted. Depending on the configuration, the M marker can be seen only by a number of out of n cameras (). An arbitrary camera () is chosen, which is considered the positioning reference.

Figure 6

The homogenous transformations involved in estimating the position of a fiducial marker placed in the environment of a calibrated camera setup.

The proposed algorithm is estimating the homogenous transformation using the noisy measurements from each of the cameras. In a similar manner to the setup calibration procedure, the transformation is expressed multiple times in terms of pose measurement (where ) using Equation (17). The is precisely known from the setup calibration.

The resulting pose is converted to translations and Euler angles and used for building the output measurement vector of Equation (3). The proposed algorithm is further applied and the result is an estimation of which has a higher degree of precision and is closer to the real value.

This approach can be used in a similar manner for measuring the relative position of multiple fiducial markers in the environment, which is very useful in alignment tasks as presented in [7].

4. Simulation Results

4.1. Monte Carlo Setup

There are two factors that contribute and affect the precision of estimating the pose of the fiducial marker. First, there are the physical and environment-related aspects, which include the optical specifications of the imaging system (the sensor and optical resolution, the focal length, the depth of field, and the field of view), the environment illumination conditions (how strong, uniform, and consistent the lighting is) and if the cameras are optimally positioned so as to achieve good viewing angles and maintain consistent accuracy along the working area. Secondly, there are factors related to the algorithms regarding how precise the pose can be estimated and how much the effect of the noise can be mitigated during the data fusion. In this paper we decouple the two types of factors and only consider the behaviour of our proposed method, so as to have a first qualitative assessment regarding the precision.

Consequently, we developed a simulation environment that aims to replicate how our method performs in a real setup, considering the noisy pose estimations it receives from the solvepnp algorithm. For an increased confidence in the results, we adopted a Monte Carlo approach in which we statistically analysed how the noise is propagated through our method and how the precision is affected. The simulation is implemented in MATLAB where positions of multiple cameras, precision gauges, and fiducial markers are virtually defined. In order to simulate the noisy input from the solvepnp, each position (that is supposed to be measured in the real environment) is disturbed with additive random noise having normal distribution. The noise is configured considering the precision limits we determined experimentally for one camera in our previous work [21]: for X and Y, m, for Z, m, and for , and , . The mean value of the noise is 0 while the standard deviation () was configured in such a way that of the noise values are within the experimental limits (inside ).

The simulation puts in place three scenarios which are additionally used to assess the different contributions between the number of cameras and the number of the fiducial markers from the gauge:

3 cameras, a precision gauge with 5 markers, and one marker whose position must be estimated;

5 cameras, a precision gauge with 3 markers, and one marker whose position must be estimated;

5 cameras, a precision gauge with 5 markers, and one marker whose position must be estimated.

The results are compared against estimating the pose using only one camera in the same environment. The Monte Carlo simulation performs 5000 iterations where, in each run and for each of the scenarios, the setup is calibrated using the gauge. The calibration is further used to estimate the pose of the fiducial marker which is compared with the true, predefined one. The and parameters are configured to 20 and 50 respectively. In a real application, the choice of these two parameters is a matter of cost optimisation, considering the computational effort, the resources available, and the required measuring frequency.

Compared with a real setup, we adopted one simplifying hypothesis. The angle of incidence of the marker relative to the camera, as we showed in [21], has an influence on the precision. Close to normal angles of incidence tend to bring more noise in the estimation. In this study, we only consider that the precision of the solvepnp algorithm to be consistent, regardless of the angle. However, in other respects, the simulation is considering the worst case scenario because of the following arguments:

Noises from different cameras and from different elements of the pose (X, Y, Z, , , and ) are considered completely uncorrelated. In real circumstances this might not be the case (e.g., the noise induced by the environment illumination which affects all the measurements in a similar manner) thus, any relaxed conditions are contributing to an increased precision of the estimation. This additional information is harnessed using the update procedure for Q and R covariance matrices (which in this case would no longer be diagonal);

The precision of one camera measurement along the Z axis is 5 times lower compared with the X and Y axis. In the simulation this is taken as it is, but in a real situation, this effect can be diminished by placing the camera setup in an optimal configuration. For instance, the measurement along the Z axis from one camera can be replaced by measurements from two cameras placed in lateral positions, for which the Z axis motion is decomposed in X and Y components that have a higher precision.

4.2. Results

Figure 7, Figure 8, Figure 9, Figure 10, Figure 11 and Figure 12 presents the simulation results for estimating X, Y, and Z coordinates and , , and orientation angles, which are given in terms of the probability density function (PDF) and the standard deviation (SD) of the error. The first plot from each Figure depicts the estimation error achieved using only one camera. The following three plots give the results achieved using the setups from each of the above-mentioned scenarios.

Figure 7

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the X coordinate.

Figure 8

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the Y coordinate.

Figure 9

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the Z coordinate.

Figure 10

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the angle.

Figure 11

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the angle.

Figure 12

The probability density function (PDF) and the standard deviation (SD) of the error for estimating the angle.

The results show that the proposed algorithm achieves an increase in precision which is close to an order of magnitude. It can also be observed that it is of greater importance to have more fiducial markers in the precision gauge instead of more cameras. In scenario #3, a slight decrease of precision is experienced in comparison to the #1 scenario. This is to be expected as each added camera is an additional noise source. However, the benefit of achieving a larger working area is far more important. In Table 1 the results are summarised and compared with regards to the limits of the variation interval where it is expected that of the errors will occur. This can be considered the precision that the positioning system is expected to achieve when using th proposed method.

Table 1

The simulation results for estimating the pose using only one camera and multiple cameras in three scenarios. The value is the boundary of the error variation interval ().

			Scenario
Pose Element	UM	One Camera	#1	#2	#3
X	(μm)	75.68	11.1	21.04	17.4
Y	(μm)	75.1	11.14	12.7	11.88
Z	(μm)	298.68	30.74	30.01	28.28
AX	(deg)	0.019	0.0016	0.0014	0.0013
AY	(deg)	0.019	0.0022	0.0024	0.0022
AZ	(deg)	0.019	0.0019	0.0018	0.0016

In order to further emphasise the simulation results achieved by the proposed method, Figure 13, Figure 14, Figure 15, Figure 16, Figure 17 and Figure 18 depict a set of examples from the 3rd scenario which show how the estimation of the state vector elements is evolving. Each of the figures presents: The noisy measurement from the five cameras, the value of the state vector computed after the initialisation procedure, the evolution of the state vector during the Kalman estimation, and the final state estimation which falls close to the true value. Although the noise in the measurements is amplified because the reconstruction of the position of the marker in different cameras, the evolution of the Kalman estimation shows a clear noise damping effect.

Figure 13

The evolution of the estimation of X.

Figure 14

The evolution of the estimation of Y.

Figure 15

The evolution of the estimation of Z.

Figure 16

The evolution of the estimation of .

Figure 17

The evolution of the estimation of .

Figure 18

The evolution of the estimation of .

5. Conclusions

With respect to the considered case study, the results show that the proposed method and algorithm have managed to successfully meet the objectives. The working area could be increased in accordance with the number of cameras in the setup. This is a decision-making procedure that needs to take into account the cost relative to the working area and the precision required. For our simulation scenarios, the precision increase was close to an order of magnitude, which was around 10–15 m for X and Y coordinates, 30 m for Z and for , , and orientation angles.

The cost is an important benefit of the system compared to other solutions like laser trackers which tend to be extremely expensive. In addition to that, our proposed method could achieve relative and simultaneous positioning of multiple fiducial markers, which supports the development of advanced applications.

Future work will include a complete analysis of the method in a real environment where all physical and algorithm-related factors are considered, and a precision comparison against other methods presented in the literature.