{'fraction_non_alphanumeric': 0.03515147990928651, 'fraction_numerical': 0.009623771588067686, 'mean_word_length': 4.806043214044564, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Optimal Pose Estimation and Covariance Analysis with Simultaneous Localization and Mapping Applications

This work provides a theoretical analysis for optimally solving the pose estimation problem using total least squares for vector observations from landmark features, which is central to applications involving simultaneous localization and mapping. First, the optimization process is formulated with observation vectors extracted from point-cloud features. Then, error-covariance expressions are derived. The attitude and position estimates obtained via the derived optimization process are proven to reach the bounds defined by the Cram\'er-Rao lower bound under the small-angle approximation of attitude errors. A fully populated observation noise-covariance matrix is assumed as the weight in the cost function to cover the most general case of the sensor uncertainty. This includes more generic correlations in the errors than previous cases involving an isotropic noise assumption. The proposed solution is verified using Monte Carlo simulations and an experiment with an actual LIDAR to validate the error-covariance analysis.

time delay between emission and return is measured. There are two basic categories of pose estimation: model-based and non-model based. Model-based approaches use a priori models of the object, and then estimate the pose of the object to match features from the model. Reference [2] states several various types of algorithms that can be used for model-based pose estimation, such as feature-based model tracking, model-to-image registration using various point-set registration algorithms, simultaneous pose and correspondence determination, template matching, contour tracking, and articulated object tracking. A survey of model-based pose estimation can be found in [3]. Non-model based approaches do not typically use any model attributes, but rather estimate an object's pose by separating the non-moving objects (e.g. background scene) from the moving object to be tracked. Non-model pose estimation is typically accomplished by identifying, locating and tracking a number of features, such as a corner or an edge, on the object [4]. Both model and non-modeled based pose estimation can be used to simultaneously estimate either single object or multiple objects in a sensor's field-of-view (FOV), typical used for SLAM applications.
There are many applications of both model and non-model based pose estimation. These include spacecraft relative navigation [5][6][7] for space rendezvous and docking purposes [8], absolute navigation of air vehicles from planar homographies [9], relative air navigation [10] in GPS-denied environments [11], robotic navigation in unknown environments [12] and GPS-denied environments [13], ground-vehicle position estimation using cameras [14], underwater-vehicle pose estimation using light beacons [15], and human pose estimation from images [16], including head position estimation [17]. The most basic steps for pose estimation first involve either using well-defined features extracted from an object or "direct methods" that not rely heavily on features. The former is usually used in conjunction with outlier elimination approaches, such as random sample consensus (RANSAC) [18], to provide more robustness to false matches. A number of algorithms exists for feature extraction, such as scale invariant feature transform (SIFT) [19] or the speeded-up robust feature (SURF). One solution for the SLAM problem involves matching the histograms extracted from the features [20]. Reference [21] utilizes a point as well as the local properties of its neighborhood, e.g. the curvature and normal direction, to align point clouds. Direct methods rely on "template matching," and use a dense image alignment approach to refine the pose estimates that aligns a given target image to a sensed image (i.e. image registration). The seminal algorithm by Lucas and Kanade, which outputs the required optical flow, is often used directly or as a basis for other approaches [22]. Once either feature extraction or direct methods have applied, pose estimation is then accomplished.
For registration of point clouds that are three-dimensional (3D) in nature, iterative closest point (ICP) algorithm implementations are commonly used [23]. However, it is well known that implementations of the ICP algorithm involve a large computational cost for arriving at the relative navigation estimates from the point clouds alone. Several researchers have implemented variants of the ICP algorithm that are more computationally efficient. Many of the variants are based on using a particular attitude parameterization [24]. For example, [25] shows an approach that uses the Caley transform, which leads to a rigorously linear least-squares solution. But, because this algorithm is based on the classical Rodrigues parameters (Gibbs vector), a degree-of-freedom is lost for 180 degree rotations. An approach using modified Rodrigues parameters (MRP) is shown in [26]. Comparisons with Euler angles-based algorithms show that the MRP algorithm is more accurate and more computationally efficient. However, it is well known that MRPs are singular for 360 degree rotations. A quaternion formulation is shown in [27]. The algorithm eliminates the unknown position vector by substraction of two images. Depths of the 3D point at each view are also eliminated by noting that they form a null vector for the matrix consisting of the point coordinates and their rotations. This leaves only the quaternion to be estimated, which is accomplished using the q method [28]. * Once the quaternion is determined, then the position vector and depths can be easily estimated. The advantage of the quaternion parameterization is that no singularities exist; however, a norm constraint must be satisfied. A dual-quaternion approach is shown in [29]. The advantages of using dual quaternions is that a single loss function can by minimized that is associated with the sum of the attitude and position errors, and no singular-condition rotations exist.
Most of the aforementioned registration algorithms do not employ a realistic sensor error-source model. For example, camera images are typically modeled using the classic pinhole model, which formulates the relationship between the coordinates of a point in 3D space and its projection onto the image plane of an ideal pinhole camera, leading to the "collinearity equations." For mathematical convenience, the projected coordinates are put in a 3 × 1 unit-norm vector, which can assume that the focal length is 1 without loss in generality. Unfortunately, the measurement noise is also transformed when the collinearity equations are converted into the unit-vector form. A simple covariance model that is valid for small FOVs has been developed [30], called the QUEST measurement model (QMM), which is a singular matrix that arises for the unit-normalization of the observations. Its basic premise is that nearly all the probability of the errors is concentrated on a very small area about the direction of the unit vector, so the sphere containing that point can be approximated by a tangent plane. Reference [31] expands upon the QMM for large-FOV cameras, which are typically used for many applications involving pose estimation. Both the QMM and large-FOV covariance models are singular, which may cause issues in estimation algorithms, but this can be overcome by replacing the singular matrix with a non-singular matrix using a rank-one update [31,32].
Properly accounting for the errors with the associated ICP equations leads to a total least squares (TLS) problem [33]. TLS is similar to standard least squares, but errors are also added to the "coefficient" matrix, also known as the "design" matrix. Non-iterative solutions of TLS problems are only provided in special cases, such as when the weighting matrix is isotropic in nature. References [34] and [35] show a TLS model for the ICP equations, but they assume that the errors have isotropic covariances. In [36], full weighting matrices are shown, and a generalized TLS-ICP is derived.
However, in the aforementioned references, analytical expressions for the estimate error-covariance is not derived. The purpose of the present paper is to formulate the pose estimation problem for the most general sensor error case, including measurement correlations that may lead to a fully populated measurement matrix, which is the most general and realistic * Note that the authors of [27] develop their own algorithm, which is equivalent to the q method. case. For isotropic errors, this paper also shows that the pose estimation problem is equivalent to a generalized version of Wahba's problem [37]. The associated estimate error-covariance matrices are also derived under the small error assumption. The error-covariance expressions are useful to quantify the overall performance of the TLS-ICP algorithms.
Also, covariance expressions for the measurement residuals are derived in this paper. These expressions are useful to remove spurious measurements under realistic issues, such as mis-association of features in the pre-processing steps.
The outline of this paper is as follows. An introductory formulation of TLS is first shown. Next, a cost function is developed based on the pose estimation problem. The necessary conditions for efficient pose estimation are then derived, and the cost function is written in an attitude-only format for the sake of simplicity in the proceeding derivations. Next, a linear approximation of the attitude error is derived to approximate the cost function within second-order terms in the attitude errors. Efficient estimation for the attitude and translation vectors are then used to derive the error-covariance of the estimates and covariance of the measurement residuals, which are beneficial for control purposes in a sense that the most accurate pose estimates are provided to the control logic. Finally, a numerical verification of the proposed TLS pose estimator is shown in the context of Monte Carlo simulations, as well as an experiment with actual LIDARs.

II. Problem Statement
The pose estimation problem is described as finding the pose of a sensor attached to a vehicle with respect to another sensor or a reference frame. The pose itself consists of two components: 1) a translation vector that connects the center of the two frames, and 2) an attitude matrix for the relative orientation of the vectors of the coordinate systems.
A geometric model relating the pose of the reference frame to the body frame is shown in Fig. 1. The associated geometric constraint acts as a measurement model. The vectors˜and˜are the coordinates of a point of interest, i.e. an observation feature of a landmark in the environment, with respect to the reference and body frames, respectively, and refers to the translation vector between the two coordinates.
The estimation approach provides an estimate of the attitude and the translation vector based on the measurement information. The estimate error, which comprises the difference between the transformed version of˜and˜, is given by (ˆ,ˆ) =˜−ˆ˜+ˆ (1) whereˆis the estimated attitude matrix, andˆis the estimated translation vector. For the case of perfect measurements, the error samples are all zero, and the problem can be solved with the measurement model of the form =ˆ−ˆ (2) where and denote the true values of the observation vectors. But in an actual applications, these errors are not zero, which leads to an optimization problem derived from a constrained maximum likelihood approach [25], given by where 3×3 is a 3 × 3 identity matrix, and is the measurement covariance matrix that accounts for errors in bothã nd˜, as well as correlations that exist between them. The determinant condition is required so thatˆis a proper orthogonal matrix. The observation errors are defined as It is assumed that zero-mean Gaussian measurement errors exist, with where E denotes expectation. The optimization problem in Eq. (3) can be shown to be related to a total least squares (TLS) problem [38].
This paper solves the pose estimation problem in Eq. (3) with the fully populated noise-covariance matrices , and using TLS. Note that although there are nine components in the attitude matrix , only three of them are independent, so the attitude estimation solution can be accomplished with a minimum of three parameters, which can be Euler angles or any other minimal attitude parameterization [24]. Regardless of the attitude parameterization used in the optimization process, the estimated attitude can be related to the true attitude using an attitude error-vector involving small roll, pitch and yaw angles, as will be seen in Section II.C.

A. Overview of Linear Least Squares and Total Least Squares
This section briefly introduces linear and total least squares, how they are related, and their differences. For a more in-depth review of the TLS, the reader is referred to [39][40][41].
Consider the measurement model of the form˜= where is an × deterministic design matrix with no errors, is the × 1 vector of unknowns,˜is the × 1 measurement vector, and is the × 1 measurement error-vector, The least squares estimate of is given by solving the following problem: subject to:ˆ=ˆ(

7)
The main underlying assumption in the statistical analysis of least squares is that˜has a Gaussian distribution with the conditional likelihood function given by where the distribution mean is denoted by and the covariance is . Because of the properties of the exponential function, maximizing the likelihood function in Eq. (8) is equivalent to minimizing the negative of the log-likelihood.
The estimate and its associated error-covariance are given bŷ One important property of linear least squares is that the estimate is unbiased [25].
As stated previously, the design matrix in the least squares measurement model in Eq. (6) has no errors. If this underlying assumption does not exist anymore, which happens in many applications, as will be seen in the SLAM problem in Section II.C, then another formulation must be used to consider the errors in the design matrix. This leads to the TLS problem, with parameters defined by˜= where Δ denotes the errors in the design matrix. Consider the following augmented matrix: The conditional likelihood function of the TLS problem is defined by where the vec operator stacks all columns of a matrix in a single column. The maximum likelihood approach for this cost function leads to the minimization of the log-likelihood function as whereˆis the estimate of , andˆ= [ˆ− 1] . A unique solution for this problem can be obtained if rank( ) = .
Also, is the covariance matrix that accounts for the errors in both˜and˜. Although the TLS solution is known to be biased, the TLS problem is proven to reach the Cramér-Rao lower bound (CRLB) [25] for the estimate error-covariance to within first-order error-terms, and therefore is an efficient estimator [42]. Closed-form solutions for the TLS problem are possible only when is an isotropic matrix.

B. Pose Estimation Sensor Model
The relation between the true vectors and is given by The matrix is the individual sensor-model design matrix and , = 1, 2, 3, are the columns of the attitude matrix .
However, a perfect measurement model is not realistic because of noise in the design matrix, as well as the observation vectors of Eq. (14). Therefore, in the actual version of the sensor model in Eq. (14) the following relation is used: where the design matrix˜and the observation vector˜have the errors of Δ and , respectively. Because the model is linear in terms of the unknowns and the translation vector , then the problem can be posed using a TLS formulation with the constraintˆ=ˆˆ−ˆ (   16) which is equivalent toˆˆ−ˆ=

C. Total Least Squares Derivation for Pose Determination
The TLS cost function is given by where is the covariance of vec(˜), and is the number of features in each sensor scan. The augmented cost function that includes the linear constraint in Eq. (17) is given by where is the Lagrange multiplier for the th constraint. Taking the partial derivative of the augmented cost function with respect to vec(ˆ), utilizing Eq. (A4), and the necessary condition for the minimization problem gives where ⊗ is the Kronecker product [43]. Using Eq. (A4), the constraint in Eq. (17) can be written as Substituting Eq. (20) into the constraint gives Solving for leads to whereˆ= (ˆ⊗ 3×3 ) (ˆ⊗ 3×3 ) . Note that in this paper,ˆis considered to be a positive-definite matrix, though it might be singular in some sensor models, such as the QMM [30]. This problem can be solved using a similar approach to the eigenvalue decomposition in [44]. Using the necessary condition forˆfrom Eq. (20), and substituting the Lagrange multiplier from Eq. (23) gives Substituting Eq. (24) into the original cost function in Eq. (18) yields This results in a formulation, satisfying the constraint in Eq. (17) in the cost function in Eq. (18). Then in terms of the observation vectors˜and˜, and fromˆ−˜ˆ=˜−ˆ˜+ˆ, the following expression is given: For simplicity in the proceeding derivations, the cost function can be written only in terms of the attitude matrix, which is known as the attitude-only cost function. For this purpose,ˆneeds to be eliminated from the cost function in Eq. (26).
The necessary condition for the translation vectorˆresults in Substituting the optimal value ofˆinto Eq. (26), the attitude-only cost function becomes where˜= are the observation vectors with their corresponding covariance shown in Eqs. (5a), (5b) and (5c), andˆ= Also it is proven in [38] that the weight matrixˆcan be derived as a function of the attitude matrix and the observation noise covariance matrices asˆ=ˆˆ−ˆ−ˆ+

D. Covariance Analysis of the Estimates and Residuals
The covariance expressions for the attitude as well as the translation and observation vector estimates are now derived. Note that the cost function can be written in terms of the attitude error, defined by , since only three independent components exist inside of the attitude matrix. The relation between the true and the estimated attitude matrix can be expressed asˆ= where [·×] denotes the cross product matrix of a vector, as shown by Eq. (A3). Using a small-angle assumption and a first-order approximation of the attitude error, the attitude estimate can be written aŝ The cost function needs be derived up to second-order in terms of the attitude error , since error-covariance expressions The following abbreviations are introduced for simplicity: This allows for the reformulation of the error-term˜−ˆ˜as Note thatˆin Eq. (31) is also a function of the attitude estimateˆand subsequently of the attitude error . As the neighboring terms are already a function of the first-order attitude error, any other term besides the attitude-error dependent ones inˆ(terms that are independent of ) are not kept for the second-order approximation of the cost function. The matrixˆis now written aŝ Further decomposition ofˆin terms of then yieldŝ The inverse ofˆis approximated by Therefore, the matrix −1 is the only portion of −1 that is not a function of the attitude error. This portion will be used for building the second-order cost function. The second summation of the cost function in Eq. (28) contains¯ˆ, which itself is a function of . The portion of¯ˆthat does not depend on the attitude error needs to be extracted. This way, the corresponding part of the cost function ignores higher-order terms. The expansion of¯ˆis given asˆ= The summations in the above equation can be abbreviated as Hence, emerges as the only term that is not a function of the attitude error. Utilizing the first-order errors in Eq. (36), the matrix in Eq. (38), and in Eq. (43a), the following approximation up to second order of the cost function is given: From the necessary condition for the extremum of the cost function with respect to , i.e.
= 0 (46) the vector estimate of the attitude-error emanates as Employing Eq. (35c) allows for expanding the above expression to At the same time, the following relation holds: Given that A = −A , as well as that −1 , and H are symmetric, the terms containing the translation vector cancel, thus yielding This attitude error now allows for the derivation of the error-covariance expressions. The estimation error-covariance of the attitude is defined as Further expansion of the individual terms leads to Employing the fact that the attitude error-covariance can be rewritten as The last two contributions in the above sum cancel out each other, thus yielding Thus, the estimate-error emerges as Using the definition of in Eq. (35c) gives The error-covariance of the translation vector within first-order of the estimation errors is given by where the fact that the estimate for the translation vector is unbiased within the first-order terms of error is used, and E{ˆ} = . Then, the translation vector error-covariance is computed as It is observed that the cross-covariance of the attitude errors and is required for the translation error-covariance, which is computed as  [38], it can be shown that the estimates of the observation vectors arê Define the following estimate errors for the observation vectors: The residual errors using Eq.
Their corresponding first-order approximations are given by and from Eq. (63), the covariances of the measurement residuals are given by Equations(69a) and (69b) can now be written as Their corresponding estimate covariances are given by

E. Isotropic Noise Covariance
The previous section provided analytical expressions for the covariance of estimates and residuals for the most generic case of observation noise covariance, which is a fully-populated symmetric positive-definite matrix as denoted in Eq. (5d). Now a particular case of previous covariance derivations is elaborated. In some sensors, there is a simplifying assumption on the distribution of noise having the same characteristics in the different space coordinates of , and .
This presumption results in a particular form of the noise covariance called isotropic. For the pose estimation problem in this work, this isotropic covariance of observation errors is denoted by where and indicate the standard deviation of noise for the sensors in the reference and body frames, respectively.
The cross-correlation between the reference and body sensor noise will be zero, as shown in the off-diagonal blocks.
For the isotropic case, the weight matrixˆin the cost function from Eq. (37) shrinks tô As a result, the second weight¯ˆof the cost function in Eq.
Then the attitude-only cost function in Eq. (28) yields The first part of the right side of Eq. (81a) is equivalent to Wahba's problem [37]. The second part of the right side of Eq. (81a) involves a weighted average of the measurements [45].
Several closed-form expressions for the attitude estimate that minimizes Eq. (81a) are possible [28]. Here the attitude matrix is itself is estimated. First, the following "attitude profile matrix" is defined:

82)
Then, its singular value decomposition is taken: The attitude estimate is given byˆ= where the det( ) det( ) term is used to ensure that the estimated attitude matrix is proper. Note that Eq. (82) is given by a rank-one perturbed matrix update.
Other solutions using the quaternion parameterization can also be determined using the approaches shown in [28].
It is noted that in the isotropic case the matrixˆin Eq. (28) is not a function of attitude matrix. Therefore, the attitude error avoids several complications in the derivation. This simplicity comes with the cost of ignoring the possible cross-covariances in the reference and body frames and assuming the same statistical properties of noise along all space coordinates. The cost function in Eq. (81a) is already second-order in terms of the attitude matrix, and there is no need for trimming the higher-order terms, which is another simplicity resulting from the isotropic assumption.
The optimal attitude from Eq. (50) yields and the Hessian matrix H is The skew-symmetric property of matrices A andĀ is employed in the above derivation, which originates from the skew-symmetric nature of the cross product matrices.
Regarding the position estimates, the estimate errors are derived from Eq. (60) as The resulting error-covariance is obtained from Eq. (65). For the isotropic case, this will be The observation vector estimates can be computed from Eq. (66a) and Eq. (66b), and for the isotropic covariance will result inˆ=˜− The estimate covariances of the observation vectors are simplified from Eqs. (75) and (76) as All of the covariance expressions reduce down to the ones derived in [38].
It is well known that at least two nonparallel vector observations must exist in order for the attitude to be observable in Wahba's standard problem [37]. It is now shown that at least three nonparallel vectors must exist for the attitude to be observable in the current pose estimation problem. For = 2, Eqs. (81c) and (81b) givē Thus, the matrix in Eq. (82) is explicitly given by The matrix must at least have rank 2 for the attitude to be observable [28]. Clearly, for the two-vector case the matrix in Eq. (99) has rank 1, which means that for this problem the attitude is not observable. This intuitively makes sense.
There are six unknowns for this problem: three for the attitude and three for the position. Each vector observation provides two pieces of information (rotations around the vector are unknown). Thus, intuitively, at least three nonparallel vectors must exist to fully solve this pose estimation problem.
Similarly, for = 2, the matrix H (the FIM) is singular and thus the attitude-error covariance is not well defined.
Clearly, H ( 1 − 2 ) = 0 3×1 . Hence, if = 2, then H is singular and the null vector of H is ( 1 − 2 ). The vectors −¯are always linearly dependent, but if = 2, then the two dependent vectors are antiparallel to each other, which makes H singular.

F. Fisher Information Matrix
From the analysis in Section II.D, the estimate error-covariances for the attitude error , the translation vector , the residuals as well as the estimate covariances for the observation vectors˜and˜have been derived. An efficiency proof for the estimate error-covariances of the attitude and translation vector is now shown based on the FIM and the CRLB defined for the estimation covariances. For an unbiased estimateˆ, the the following inequality exists [25]: The term inside of the expectation shows the Hessian of the the negative log-likelihood function, which is given in Eq. (44). For an optimal estimator, the equality in the Eq. (102) is given, and the estimator is efficient. The Hessian of the second-order approximated cost function is the FIM. From Eqs. (36) and Eq. (57), the following are given: The second-order cost function involving is now given by The FIM, denoted by , will be The block matrices of the FIM are The inverse of FIM, denoted by F , is given by The F 11 block of this matrix is calculated by the Sherman-Morrison-Woodbury lemma [46]: This has already been proven in Eq. (56), which is the CRLB for the attitude error. The term F 22 is also derived by using matrix inversion lemma as Using Eq. (56) leads to This proves the CRLB for covariance of translation vector estimate. Note that from Eq. (56) it can be concluded that the CRLB holds for the attitude estimate because the estimate error-covariance is equal to the inverse of the Hessian of the cost function in Eq. (44).

III. Simulated and Experimental Validations
This section provides a simulated and experimental validations of the TLS solution for pose estimation, as well as the associated covariance expressions. For the simulated validation, Monte Carlo simulations are introduced and employed to perform several realizations of the noise in the observation data, and interpret the results of the estimation in the sense of comparing estimation errors and estimation 3 bounds, derived from the estimate error-covariances. Also, references [47] and [48] elaborate on this simulation and showcases that the covariance expressions are accurate through Monte Carlo simulations. The experimental validation is done using a LIDAR system with an accurate ground truth.
The true observation vectors are generated by the constraint in Eq. (14). bounding their corresponding estimate errors or residuals. Artificial noise is generated from a Gaussian distribution with zero mean and covariance of , and to produce˜and˜samples. The covariance matrices of the measurements , and are generated randomly with a signal-to-noise ratio of around 10 −4 . Note that by default, the measurement covariance is selected to be positive-definite while being random. Singularities in the measurement covariance matrix can be handled if they exist, but this is not the focus of this paper.

B. Experimental Results
An actual experiment with a Velodyne HDL-32 LIDAR is performed, in which a series of scans are taken from at least three basketballs as simple features in the LIDAR proximity. The pre-processing steps and the general estimation evaluation for these experiments are analyzed in section III.B. Two types of sensors are employed in these experiments; first the Velodyne VLP-32 LIDAR is used to verify the TLS solution for the point-cloud registration problem, and second a multi-camera Optitrack system is utilized, which has much better accuracy than the LIDAR in terms of tracking the 3-dimensional positions. The vision system's accuracy is sub-millimeter for each , and dimension, which is orders of magnitude better than the estimated position from the LIDAR derived estimates. The measurement covariance for the LIDAR is obtained using a static configuration; the mean and covariance are computed using a simple sampling approach. The Optitrack system is utilized as ground truth for the pose estimates from the LIDAR data.
The minimum number of three features is selected as the centroids of the three spheres in the LIDAR field of view.
Also, the features should be visible in at least three cameras from the Optitrack system to be able to accurately capture the ground truth data. This ground truth data is required for the LIDAR pose and the observation vectors provided by the Optitrack system. For this experimental setup, the LIDAR is static and takes point cloud measurements from surrounding basketballs, which are also static with respect to an inertial coordinate frame on the floor.
In the following sections, the pre-processing steps are discussed in detail.
The experimental results for the point-cloud registration problem in Eq. (26) are discussed here.
A picture of the experimental environment is shown in Fig. 5(a). The cylinders are there to avoid the ground-floor outliers in LIDAR raw data. The rviz visualization of the experiments is shown in Fig. 5(b).
The Optitrack system follows the rays from a series of markers attached to the objects of interest in the environment.
The red-green-blue rods show the coordinate frames on each sphere as well as the LIDAR. The scattered points in the background are the outlier and noisy data from other unwanted objects. In the case of tracking the orientation of an object, the Optitrack software needs at least three markers attached to the object. For each basketball and the LIDAR, four markers are attached to satisfy the tracking requirements of the Optitrack software.
In this experimental setup, the LIDAR and the basketballs are not moving with respect to a pre-determined inertial frame on the floor. It is noted that the ground plane appears in the LIDAR scans and needs to be removed as a pre-processing step to reject the unwanted data for our pose estimation purpose. The pre-processing steps are elaborated in Fig. 6.
The results of the TLS pose estimation for this scenario are shown in Figs. 7(a), 7(b), as well as Figs. 8(a) to 8(d).
As seen in the attitude-error plots in Fig. 7(a), the angle estimation errors are bounded within limits defined by the 3 bounds, extracted from the attitude error-covariance in Eq. (56). It is expected that approximately 3 out of 1,000 of the errors should be outside of the 3 bounds. It is seen that there are more than 3 outside of their bounds, which may be due to calibration errors. Still, reasonable performance is given.

IV. Conclusions
This paper develops an analytical solution for an efficient pose estimation problem within the first-order approximation of estimate errors. Pose estimation is central to simultaneous localization and mapping (SLAM) problems. Efficient estimation makes the controller policy easier to track the desired signals to obtain a more accurate estimate of the states. The static SLAM problem is shown to be solved as a total least squares (TLS) problem. A quadratic cost function based on the TLS formulation is introduced for taking into account the attitude matrix and translation vector.
The weight matrix in the cost function is extracted from the most generic positive-definite fully populated matrix to include the correlations between the observation vectors in the most general case. The cost function is then written in an attitude-only format. The error-covariance expression of the attitude error is provided within the small-angle assumption. This assumption leads to a second-order approximation of the cost function in terms of the attitude error. Optitrack multi-camera system is utilized for scanning the ground truth data of the sensor's position and orientation.
The estimate error-covariances are verified to their corresponding estimate or residual errors in the actual experimental setup, which validates the derived analytical expressions.

Appendix
A list of some preliminary equations used in the paper is shown here. For the cross product of two 3 × 1 vectors and , the following relation is given: The cross product matrix of a vector is a skew-symmetric matrix, so that where [ ×] denotes the 3 × 3 cross product matrix constructed from the vector as The Kronecker product [43] also can be combined with the vec operator in the following identity, where the notation vec(·) [49] for an × matrix results in an ( ) × 1 matrix vec( ) that is constructed by stacking the columns of : = ( ⊗ × )vec( ) (A4)