# Use of Tikhonov Regularization to Improve the Accuracy of Position Estimates in Inertial Navigation

Authors: Tuukka Nieminen; Jari Kangas; Lauri Kettunen
Journal: International Journal of Navigation and Observation
 (2011)

---

## Abstract

Inertial navigation problems are often understood as initial value problems. However, there are many applications where boundary value problems naturally arise. In these situations, it has been shown that the finite element method can be efficiently used to compute accurate position and velocity estimates. We will propose that finite element method complemented with Tikhonov regularization—a basic tool for inverse problems—is a powerful combination for further accuracy improvements. The proposed method provides a straightforward way to exploit prior information of various types and is subject to rigorous optimality results. Use and accuracy of the proposed method are demonstrated with examples.


---

## Body

## 1. Introduction

The terminertial navigation is often associated with the term initial value problem (IVP). This kind of an association is justified in many cases, but not always: there are situations, where an inertial navigation problem may just as naturally be posed as a boundary value problem (BVP) [1]. In case of a BVP, all measurements within the interesting time interval are exploited, whereas an IVP only exploits measurements up to a certain time within the time interval.We will discuss BVPs concerning the position of the object, given the accelerations in the navigation frame. Instead of the “full equations”, where rotation of Earth is taken into account, we consider “simplified equations”, where this is neglected [1]. In this case, to obtain the position (p) of the object, we need to solve the following problem: given a(t):ℝ→ℝ3, find p(t):ℝ→ℝ3 such that r satisfies
(1)d2p(t)dt2=a(t).
The problem of resolving accelerations a(t) given the specific force and angular rate measurements is a different matter, not considered herein. What makes (1) a BVP is that the necessary constraints are not given at same time [2]. More specifically, we will consider two-point BVPs where the necessary constraints are given at the beginning and at the end of the interesting time interval [3].In [1], a custom-made FEM model is derived to treat two-point BVPs of the form (1) with various choices of boundary conditions. The resulting system of linear equations is shown in [1] to be of the form
(2)[Ax000Ay000Az][rxryrz]=[bxbybz].
In (2), rx,ry,rz∈ℝN, and rx, for example, is a vector containing the x-coordinate of the object at N different time instances. Vectors bx, by, and bz are determined by a(t). Submatrices Ax, Ay, and Az are symmetric, positive definite and with the basis functions considered in [1], tridiagonal N×N matrices. In the following, a shorthand notation for (2) is used. It is written in the form
(3)Ar=b+ϵ,
where vector ϵ∈ℝ3N is added to emphasize the presence of measurement errors, generally of unknown properties. Based on the above discussion, the matrix A∈ℝ3N×3N is known to be nonsingular. For a 2D problem, the dimensions of system (3) are 2N and, respectively, for 1D, N. Unless otherwise noted, the following discussion assumes dimensions to be 3N.The goal of this paper is to estimater as accurately as possible, given A, b, and additional information of some form. Sometimes, also some properties of ϵ can be known but in general, we do not make such an assumption. We will be using the term “additional information” throughout this paper, so let us now clarify this concept: “additional information” refers to all available information that can be represented in the linear form
(4)Dr=e+θ,
where D∈ℝM×3N and e∈ℝM. In (4), the term θ denotes uncertainty, just like ϵ in (3). The integer M is independent of N and indicates how many individual equations the additional information contains. Notice that properties of D are generally unknown.In [1], problems of the form (3) are discussed. There, it is assumed that ϵ contains mainly deterministic error components, caused by deficient sensors. Then, one can exploit provided additional information of type (4) by modeling errors of these deficient sensors, which results in smaller position and velocity errors via reduction of ∥ϵ∥2 (where index 2 refers to the 2-norm). While this approach was shown to work well in many cases, it is adequate only for situations where the types of the most significant sensor errors are known. In practice, the approach proposed in [1] also limits the attainable accuracy in situations where M is significantly larger than the number of parameters in the chosen error model. The motivation of this paper is to present a new technique to exploit additional information, without the limitations of the technique proposed in [1].The technique proposed herein is based onTikhonov regularization [4–6], independently developed also by Phillips [7, 8]. As the name suggests, the basic idea of the method is to regularize the solution r of (3). This is required in order to reduce the effects of the error term ϵ. Tikhonov regularization has been studied extensively during the last few decades, mainly in the field of inverse problems [8]. Thus, the properties of the method are well known. In the statistical literature, Tikhonov regularization is known as ridge regression [8–10].We apply a tool of inverse problem theory to inertial navigation, becauseinertial navigation problems are, in fact, inverse problems. That is, we are basically resolving the time-parametrized trajectory of an object based on erroneous observations of specific force and angular rate. This reasoning is supported also by the fact that inertial navigation problems are particularly sensitive to measurement errors, which is the hallmark of inverse problems.This paper is organized as follows: first, we will discuss the background of this study in Section1.1 and make some remarks about this study in Section 1.2. In Section 2, Tikhonov regularization is presented. Some practical aspects of the proposed method are discussed in Section 3. Sections 4 and 5 are devoted to numerical examples and the conclusions are presented in Section 6.


### 1.1. Background

Problems resembling the ones considered here have previously been resolved using the means offixed interval smoothing [11–14]. The most frequently used methods of solving these problems are two-filter smoother [15] and Rauch-Tung-Striebel smoother [16]. These are exploited, for example, in [14, 17–19]. In both cases, differential equation (1) is realized as an IVP. The basic idea of these methods is to run the filter in the forward direction as a “predictor” in phase one and then to run the filter in the backward direction as a “corrector” in phase two. In this approach, each additional constraint is assumed to be associated to a single time instance.The proposed BVP formulation does not make a distinction between forward and backward directions. Instead of advancing one step at a time, the BVP formulation considers the whole time period, and the solution is obtained in one phase. In addition to the different realization of (1), the key difference is the form of (4), which allows additional information to involve an arbitrary number of time instances. This makes it possible and straightforward to exploit wide-ranging types of additional information. Another particularly useful feature of the proposed method is its capability of finding a reasonable balance between (3) and (4) without knowledge of ∥ϵ∥2 and ∥θ∥2. This property is discussed in Section 2.3.The similarity between fixed interval smoothing and the BVP approach is that they can exploit information with varying reliability and have similar optimality results. Some optimality results of the proposed method are presented in Section2.2. Although there are situations, where both methods could be exploited, we will not compare these methods quantitatively.


### 1.2. Basic Assumptions and Notation

We assume that some additional information is always available, that is,M>0. If this is not the case, the presented method will reduce back to the one considered already in [1]. We will insist on additional information given in the form (4). A more general setting can be found, for example, in [20].From time to time, we will need to print the entries of matrices and vectors. They are denotedAi,j (for matrix A) and bj (for vector b), where i and j are positive integers within the allowed range. Symbols i and j are reserved only for this use. Similarly, notation ti refers to the ith time instance, where the time starts at t1 and ends at tN. Step size is defined as hi=ti+1-ti, where 1≤i≤N-1. Without loosing generality, for simplicity we treat the time step as if it was a constant. Finally, a normally distributed random variable x with expectation (or mean) value E(x) and variance σ2 is denoted as x~𝒩(E(x),σ2).


## 1.1. Background

Problems resembling the ones considered here have previously been resolved using the means offixed interval smoothing [11–14]. The most frequently used methods of solving these problems are two-filter smoother [15] and Rauch-Tung-Striebel smoother [16]. These are exploited, for example, in [14, 17–19]. In both cases, differential equation (1) is realized as an IVP. The basic idea of these methods is to run the filter in the forward direction as a “predictor” in phase one and then to run the filter in the backward direction as a “corrector” in phase two. In this approach, each additional constraint is assumed to be associated to a single time instance.The proposed BVP formulation does not make a distinction between forward and backward directions. Instead of advancing one step at a time, the BVP formulation considers the whole time period, and the solution is obtained in one phase. In addition to the different realization of (1), the key difference is the form of (4), which allows additional information to involve an arbitrary number of time instances. This makes it possible and straightforward to exploit wide-ranging types of additional information. Another particularly useful feature of the proposed method is its capability of finding a reasonable balance between (3) and (4) without knowledge of ∥ϵ∥2 and ∥θ∥2. This property is discussed in Section 2.3.The similarity between fixed interval smoothing and the BVP approach is that they can exploit information with varying reliability and have similar optimality results. Some optimality results of the proposed method are presented in Section2.2. Although there are situations, where both methods could be exploited, we will not compare these methods quantitatively.


## 1.2. Basic Assumptions and Notation

We assume that some additional information is always available, that is,M>0. If this is not the case, the presented method will reduce back to the one considered already in [1]. We will insist on additional information given in the form (4). A more general setting can be found, for example, in [20].From time to time, we will need to print the entries of matrices and vectors. They are denotedAi,j (for matrix A) and bj (for vector b), where i and j are positive integers within the allowed range. Symbols i and j are reserved only for this use. Similarly, notation ti refers to the ith time instance, where the time starts at t1 and ends at tN. Step size is defined as hi=ti+1-ti, where 1≤i≤N-1. Without loosing generality, for simplicity we treat the time step as if it was a constant. Finally, a normally distributed random variable x with expectation (or mean) value E(x) and variance σ2 is denoted as x~𝒩(E(x),σ2).


## 2. An Introduction to Tikhonov Regularization

Tikhonov regularized solution of (3) and (4) is the solution of
(5)rλ=argminr⁡‖Ar-b‖22+λ‖Dr-e‖22,
where λ>0 is called the regularization parameter. It is used to weight (3) with respect to (4). Value λ=1, for example, indicates that both equations are weighted equally. With λ→0, ∥Arλ-b∥→0 and the regularized solution rλ approaches the solution of (3) (with ϵ=0). Respectively, with λ→∞, the regularized solution rλ approaches a solution satisfying (4).Let us point out that in the literature, Tikhonov regularization is seldom presented in the form (5). Most often, it is assumed that the dimensions of A and D are equal. Moreover, in many formulations, D is set to be a difference operator or the identity matrix (I) and/or e=0 [8, 21, 22].


### 2.1. Existence and Uniqueness of the Solution

The questions regarding problem (5) are(1)
is there a solution to (5)?(2)
is the solution unique?(3)
how can one find the solution?Answer to question one follows easily from the fact that the cost function of (5) is nonnegative for every λ>0, indicating that the minimization problem has at least one solution. To answer questions two and three, let us denote the value of the cost function by
(6)Qλ(r)=‖Ar-b‖22+λ‖Dr-e‖22.
As known, function Qλ(r) has a local extremum in a critical point of (6), in other words, at a point where the Gâteaux derivative
(7)dduQλ(r+uw)|u=0=0∀w≠0,
where u∈ℝ and w∈ℝ3N. To find critical point(s) of (6), let us substitute (6) into (7). For the first term of (6), we get
(8)ddu‖A(r+uw)-b‖22|u=0=ddu〈Ar+uAw-b,Ar+uAw-b〉|u=0=2〈Ar-b,Aw〉,
where 〈·,·〉 denotes an inner product. Similarly for the latter term of (6),
(9)ddu‖D(r+uw)-e‖22|u=0=2〈Dr-e,Dw〉.
By substituting these into (7) and dividing by 2, we have
(10)〈Ar-b,Aw〉+λ〈Dr-e,Dw〉=0.
By exploiting the properties of the inner product, it follows that
(11)〈(ATA+λDTD)r-(ATb+λDTe),w〉=0.
Since this has to hold for all w≠0, we finally get
(12)rλ=(ATA+λDTD)-1(ATb+λDTe).
Critical point of (6) is unique as long as the matrix (ATA+λDTD) is nonsingular. This is true if and only if the null spaces (i.e., kernels) of matrices A and D intersect trivially: ker(A)∩ker(D)={0} [8]. Given that A is nonsingular, this is the case independently of D. Thus, the critical point of (6) is always unique. Because the minimization problem (5) is known to have a solution, the found critical point must be located at the global minimum of the cost function Qλ(r).Alternatively, the solution of (5) can be obtained by finding the least squares solution of
(13)[Aλ1/2D]rλ=[bλ1/2e],
which is equivalent to (12). For later reference, system (12) is called normal equations and (13) augmented system.


### 2.2. Optimality Results ofrλ

Let us now examine the proposed method from the Bayesian point of view. For this, consider the special case of (5), where M=3N and D=I:
(14)rλ=argminr⁡‖Ar-b‖22+λ‖r-e‖22.
Furthermore, consider that ϵ~𝒩(0,σϵ2I) and r~𝒩(e,σθ2I), where σϵ2 and σθ2 represent variances of ϵ and θ, respectively. Here, r is considered to be a random variable. Then, according to [23], the maximum a posteriori estimator of r is the solution of
(15)argmin⁡r⁡‖Ar-b‖22+σϵ2σθ2‖r-e‖22.
That is, the choice λ=σϵ2/σθ2 yields the best estimate of r in the sense that the likelihood of r is maximized.Similar optimality results are known also for more general situations, where for example,ϵ~𝒩(0,σϵ2CCT). For an extensive discussion of these situations, see [8, 24–26]. To the best of our knowledge, it is an open question whether similar results apply in case of the general form of (5).


### 2.3. The Choice ofλ

In this section, we will provide an introduction to the most relevant systematical methods of findingλ.According to [8], parameter-choice methods can be divided into two classes depending on their assumptions about the error norm ∥ϵ∥2:(1)
methods based on knowledge of∥ϵ∥2,(2)
methods that do not require∥ϵ∥2.Out of these two classes of methods, our main interest is in class two. The reason for this is that knowledge of∥ϵ∥2 is, in general, a too strict requirement to make. For reference, the most widespread parameter-choice method of class one is the discrepancy principle [8].In class two, there are three popular and widely used methods. These are thequasioptimality criterion [8, 27], generalized cross-validation [8, 28], and L-curve criterion [8, 29, 30]. Let us next take a closer look at the L-curve method, which will be used here to choose the value of λ.The reason for this choice is that the L-curve criterion is, in fact, accessible and a quite intuitive tool for the selection ofλ. In the heart of this method is the shape of a certain curve (see Figure 1), which is parametrized as
(16)[ζ(λ),η(λ)]=[log⁡‖Arλ-b‖2,log⁡‖Drλ-e‖2].
The idea of the L-curve criterion is to choose λ in such a way that there is a “good balance” between the values of the two norms in (5). In other words, balance between ∥ϵ∥2 and ∥θ∥2. Specifically, the L-curve criterion chooses the value of λ maximizing the curvature
(17)κ(λ)=ζ̇η̈-ζ̈η̇[(ζ̇)2+(η̇)2]3/2
of the curve (16). In (17), the dots represent first (̇) and second (̈) derivatives of ζ and η with respect to λ. The location of the point with maximum curvature is demonstrated in Figure 1. In practice, a suitable one dimensional optimization routine, such as quasi-Newton, can be used to find this point. For a more complete discussion of the reasoning behind the L-curve method, see, for example, [8]. It would also be interesting to compare existing parameter-choice methods in navigation applications, but this does not fit the scope of this paper.Figure 1
Generic form of the L-curve.


## 2.1. Existence and Uniqueness of the Solution

The questions regarding problem (5) are(1)
is there a solution to (5)?(2)
is the solution unique?(3)
how can one find the solution?Answer to question one follows easily from the fact that the cost function of (5) is nonnegative for every λ>0, indicating that the minimization problem has at least one solution. To answer questions two and three, let us denote the value of the cost function by
(6)Qλ(r)=‖Ar-b‖22+λ‖Dr-e‖22.
As known, function Qλ(r) has a local extremum in a critical point of (6), in other words, at a point where the Gâteaux derivative
(7)dduQλ(r+uw)|u=0=0∀w≠0,
where u∈ℝ and w∈ℝ3N. To find critical point(s) of (6), let us substitute (6) into (7). For the first term of (6), we get
(8)ddu‖A(r+uw)-b‖22|u=0=ddu〈Ar+uAw-b,Ar+uAw-b〉|u=0=2〈Ar-b,Aw〉,
where 〈·,·〉 denotes an inner product. Similarly for the latter term of (6),
(9)ddu‖D(r+uw)-e‖22|u=0=2〈Dr-e,Dw〉.
By substituting these into (7) and dividing by 2, we have
(10)〈Ar-b,Aw〉+λ〈Dr-e,Dw〉=0.
By exploiting the properties of the inner product, it follows that
(11)〈(ATA+λDTD)r-(ATb+λDTe),w〉=0.
Since this has to hold for all w≠0, we finally get
(12)rλ=(ATA+λDTD)-1(ATb+λDTe).
Critical point of (6) is unique as long as the matrix (ATA+λDTD) is nonsingular. This is true if and only if the null spaces (i.e., kernels) of matrices A and D intersect trivially: ker(A)∩ker(D)={0} [8]. Given that A is nonsingular, this is the case independently of D. Thus, the critical point of (6) is always unique. Because the minimization problem (5) is known to have a solution, the found critical point must be located at the global minimum of the cost function Qλ(r).Alternatively, the solution of (5) can be obtained by finding the least squares solution of
(13)[Aλ1/2D]rλ=[bλ1/2e],
which is equivalent to (12). For later reference, system (12) is called normal equations and (13) augmented system.


## 2.2. Optimality Results ofrλ

Let us now examine the proposed method from the Bayesian point of view. For this, consider the special case of (5), where M=3N and D=I:
(14)rλ=argminr⁡‖Ar-b‖22+λ‖r-e‖22.
Furthermore, consider that ϵ~𝒩(0,σϵ2I) and r~𝒩(e,σθ2I), where σϵ2 and σθ2 represent variances of ϵ and θ, respectively. Here, r is considered to be a random variable. Then, according to [23], the maximum a posteriori estimator of r is the solution of
(15)argmin⁡r⁡‖Ar-b‖22+σϵ2σθ2‖r-e‖22.
That is, the choice λ=σϵ2/σθ2 yields the best estimate of r in the sense that the likelihood of r is maximized.Similar optimality results are known also for more general situations, where for example,ϵ~𝒩(0,σϵ2CCT). For an extensive discussion of these situations, see [8, 24–26]. To the best of our knowledge, it is an open question whether similar results apply in case of the general form of (5).


## 2.3. The Choice ofλ

In this section, we will provide an introduction to the most relevant systematical methods of findingλ.According to [8], parameter-choice methods can be divided into two classes depending on their assumptions about the error norm ∥ϵ∥2:(1)
methods based on knowledge of∥ϵ∥2,(2)
methods that do not require∥ϵ∥2.Out of these two classes of methods, our main interest is in class two. The reason for this is that knowledge of∥ϵ∥2 is, in general, a too strict requirement to make. For reference, the most widespread parameter-choice method of class one is the discrepancy principle [8].In class two, there are three popular and widely used methods. These are thequasioptimality criterion [8, 27], generalized cross-validation [8, 28], and L-curve criterion [8, 29, 30]. Let us next take a closer look at the L-curve method, which will be used here to choose the value of λ.The reason for this choice is that the L-curve criterion is, in fact, accessible and a quite intuitive tool for the selection ofλ. In the heart of this method is the shape of a certain curve (see Figure 1), which is parametrized as
(16)[ζ(λ),η(λ)]=[log⁡‖Arλ-b‖2,log⁡‖Drλ-e‖2].
The idea of the L-curve criterion is to choose λ in such a way that there is a “good balance” between the values of the two norms in (5). In other words, balance between ∥ϵ∥2 and ∥θ∥2. Specifically, the L-curve criterion chooses the value of λ maximizing the curvature
(17)κ(λ)=ζ̇η̈-ζ̈η̇[(ζ̇)2+(η̇)2]3/2
of the curve (16). In (17), the dots represent first (̇) and second (̈) derivatives of ζ and η with respect to λ. The location of the point with maximum curvature is demonstrated in Figure 1. In practice, a suitable one dimensional optimization routine, such as quasi-Newton, can be used to find this point. For a more complete discussion of the reasoning behind the L-curve method, see, for example, [8]. It would also be interesting to compare existing parameter-choice methods in navigation applications, but this does not fit the scope of this paper.Figure 1
Generic form of the L-curve.


## 3. Practical Aspects of the Proposed Method

In this section, we will discuss some practical aspects of Tikhonov regularization with L-curve criterion, when it is applied to inertial navigation. First, we will give some insight on the choice ofD and then, discuss the numerical aspects of the problem.


### 3.1. The Choice ofD

As mentioned in the introduction,D is determined by the application at hand. Indeed, an application with M position constraints, for example, results in different realization of D than an application with M velocity constraints would. Both of these are examples of constraints that bound position estimates only locally. Average velocity over longer period of time, trajectory of the object, or possible symmetry in the computed result are examples of constraints bounding an arbitrary number of time instances.We will now present some examples to demonstrate exploitation of possible constraints. For readability and notational convenience, these examples are one dimensional. It is, however, straightforward to generalize the presented examples to the three dimensions. In the examples, only the nonzero elements of the corresponding row ofD and e are presented:(I)
(position constraint)rj~𝒩(E(rj),σi2):
(18)Di,j=1σi2,ei=E(rj)σi2,(II)
(velocity constraint)ṙj~𝒩(E(ṙj),σi2):
(19)Di,j-1=-1(2hσi2),Di,j+1=1(2hσi2),ei=E(ṙj)σi2,(III)
position vectorr is symmetric when mirrored around the middle element (where N is odd) with pointwise variance σi2 (here, j=N-i+1):
(20)Di,i=-1σi2,Di,j=1σi2,ei=0.∀i∈[1,N2)In these examples, varianceσi2, ∀i≤M, is used to give each constraint a suitable weight, similar to the weighted least squares method [31], assuming, of course, that θi are independent.The fact that∥ϵ∥2 is unknown and L-curve criterion is used to find λ gives rise to an interesting observation regarding the variance. In addition to its traditional interpretation, σi2 can be treated as a measure of “relative” reliability of each constraint with respect to other constraints. The L-curve criterion is then used to find the “absolute” variances, given by λσi2.In this section, we have only considered the situation where theM equations within (4) have different weights. Since (3) contains the measured accelerations and the required boundary conditions [1], the N equations within (3) can also have different significance. In this kind of a situation, one can introduce a diagonal weighting matrix W, Wi,i>0 and multiply both sides of (3) with it. In this paper, however, it is assumed that W=I.


### 3.2. Computational Aspects

The overall performance of the proposed method is determined by whetherλ has to be solved or not. Namely, if we do not know it, it must be estimated as well, which requires that (5) must be solved for multiple different values of λ. The worse our initial guess for λ is, the harder it gets to find a reasonable value for it. While this “brute force” strategy might be a reasonable choice if the computations are made on a modern desktop computer, it is probably not a practical thing to do on a platform with limited resources. This is why we suggest that when possible, a suitable value of λ should be estimated beforehand, using either simulations or preferably, test measurements. After this, we can expect reasonable accuracy in similar situations, at least where the same type of measurement equipment is used. In example two, we use this strategy to avoid the re-evaluation of λ for each measurement run.Independently ofλ, problem (5) must be solved at least once. In order to avoid possible accuracy and/or performance issues, some attention should be paid. Although normal equations (12) and the augmented system (13) are mathematically equivalent, they are quite different from the numerical solution point of view. Namely, solving the system with normal equations is known to be inaccurate due to round-off errors when forming matrices ATA and λDTD. The augmented system, on the other hand, can be accurately solved for with QR or SVD decomposition. There are specific solvers available also for sparse augmented systems [32].Considering the computational complexity, the normal equations seem quite attractive: with a tridiagonal and symmetric matrixA [1], it is easy to see that ATA=A2 will be a symmetric matrix with a bandwidth of five. The structure of the regularization matrix D is, however, arbitrary in general. Yet in many situations, it will also be a banded matrix, like in the case of constraints (18) and (19). In case of (20), the situation looks worse, but significantly smaller bandwidth can be obtained by reordering the equations. In case of the normal equations, the resulting system has dimensions 3N×3N (for general three-dimensional problem) independently of M.Consider a problem leading to a reasonably small bandwidthp∈ℕ in case of the normal equations. For these situations, we propose an iterative refinement-based solution method similar to the corrected seminormal equations (CSNE) method [32]. Traditionally, CSNE is based on the use of a numerical approximation of the matrix Ã=ATA+λLTL, computed with QR decomposition. In this case (p≪3N), we can, however, compute elements of Ã analytically with negligible round-off errors. Thus, no QR decomposition is required and the corresponding algorithm for finding r isrnew⟵0,rold⟵1While∥rnew-rold∥>δdorold⟵rnewδr⟵[bλe]-[AλD]roldrupdate⟵Ã-1([ATλDT]δr)rnew⟵rold+rupdateendWhilewith stopping criterionδ [32]. Notice that the solution of the involved matrix equation can be carried out with linear time complexity, as it requires 3Np2+24Np+3N flops [31]. In many occasions, one refinement step is enough, and more than three refinement steps are seldom required. For problems with “large” p, this approach is not practical, and solvers designed for augmented systems should be used instead.


## 3.1. The Choice ofD

As mentioned in the introduction,D is determined by the application at hand. Indeed, an application with M position constraints, for example, results in different realization of D than an application with M velocity constraints would. Both of these are examples of constraints that bound position estimates only locally. Average velocity over longer period of time, trajectory of the object, or possible symmetry in the computed result are examples of constraints bounding an arbitrary number of time instances.We will now present some examples to demonstrate exploitation of possible constraints. For readability and notational convenience, these examples are one dimensional. It is, however, straightforward to generalize the presented examples to the three dimensions. In the examples, only the nonzero elements of the corresponding row ofD and e are presented:(I)
(position constraint)rj~𝒩(E(rj),σi2):
(18)Di,j=1σi2,ei=E(rj)σi2,(II)
(velocity constraint)ṙj~𝒩(E(ṙj),σi2):
(19)Di,j-1=-1(2hσi2),Di,j+1=1(2hσi2),ei=E(ṙj)σi2,(III)
position vectorr is symmetric when mirrored around the middle element (where N is odd) with pointwise variance σi2 (here, j=N-i+1):
(20)Di,i=-1σi2,Di,j=1σi2,ei=0.∀i∈[1,N2)In these examples, varianceσi2, ∀i≤M, is used to give each constraint a suitable weight, similar to the weighted least squares method [31], assuming, of course, that θi are independent.The fact that∥ϵ∥2 is unknown and L-curve criterion is used to find λ gives rise to an interesting observation regarding the variance. In addition to its traditional interpretation, σi2 can be treated as a measure of “relative” reliability of each constraint with respect to other constraints. The L-curve criterion is then used to find the “absolute” variances, given by λσi2.In this section, we have only considered the situation where theM equations within (4) have different weights. Since (3) contains the measured accelerations and the required boundary conditions [1], the N equations within (3) can also have different significance. In this kind of a situation, one can introduce a diagonal weighting matrix W, Wi,i>0 and multiply both sides of (3) with it. In this paper, however, it is assumed that W=I.


## 3.2. Computational Aspects

The overall performance of the proposed method is determined by whetherλ has to be solved or not. Namely, if we do not know it, it must be estimated as well, which requires that (5) must be solved for multiple different values of λ. The worse our initial guess for λ is, the harder it gets to find a reasonable value for it. While this “brute force” strategy might be a reasonable choice if the computations are made on a modern desktop computer, it is probably not a practical thing to do on a platform with limited resources. This is why we suggest that when possible, a suitable value of λ should be estimated beforehand, using either simulations or preferably, test measurements. After this, we can expect reasonable accuracy in similar situations, at least where the same type of measurement equipment is used. In example two, we use this strategy to avoid the re-evaluation of λ for each measurement run.Independently ofλ, problem (5) must be solved at least once. In order to avoid possible accuracy and/or performance issues, some attention should be paid. Although normal equations (12) and the augmented system (13) are mathematically equivalent, they are quite different from the numerical solution point of view. Namely, solving the system with normal equations is known to be inaccurate due to round-off errors when forming matrices ATA and λDTD. The augmented system, on the other hand, can be accurately solved for with QR or SVD decomposition. There are specific solvers available also for sparse augmented systems [32].Considering the computational complexity, the normal equations seem quite attractive: with a tridiagonal and symmetric matrixA [1], it is easy to see that ATA=A2 will be a symmetric matrix with a bandwidth of five. The structure of the regularization matrix D is, however, arbitrary in general. Yet in many situations, it will also be a banded matrix, like in the case of constraints (18) and (19). In case of (20), the situation looks worse, but significantly smaller bandwidth can be obtained by reordering the equations. In case of the normal equations, the resulting system has dimensions 3N×3N (for general three-dimensional problem) independently of M.Consider a problem leading to a reasonably small bandwidthp∈ℕ in case of the normal equations. For these situations, we propose an iterative refinement-based solution method similar to the corrected seminormal equations (CSNE) method [32]. Traditionally, CSNE is based on the use of a numerical approximation of the matrix Ã=ATA+λLTL, computed with QR decomposition. In this case (p≪3N), we can, however, compute elements of Ã analytically with negligible round-off errors. Thus, no QR decomposition is required and the corresponding algorithm for finding r isrnew⟵0,rold⟵1While∥rnew-rold∥>δdorold⟵rnewδr⟵[bλe]-[AλD]roldrupdate⟵Ã-1([ATλDT]δr)rnew⟵rold+rupdateendWhilewith stopping criterionδ [32]. Notice that the solution of the involved matrix equation can be carried out with linear time complexity, as it requires 3Np2+24Np+3N flops [31]. In many occasions, one refinement step is enough, and more than three refinement steps are seldom required. For problems with “large” p, this approach is not practical, and solvers designed for augmented systems should be used instead.


## 4. Example 1

In this section, we will work out a simple one dimensional example demonstrating the use of the Tikhonov regularization method. The object of this example is to compare three different solution strategies, all of which are based on the finite element method:(I)
tikhonov regularization based on finite element method with the position fixed at both ends,(II)
finite element method with the position fixed at both ends,(III)
finite element method with the position fixed at both ends, where measurement error is modeled as described in [1].


### 4.1. Setting Up the Example

Let us generate the reference positionp(t), t∈[0,10] s as a fifth-order polynomial satisfying the following six conditions:
(21)p(0)=0m,ṗ(0)=0m/s,p(5)=-10m,ṗ(5)=0m/s,p(10)=10m,ṗ(10)=0m/s.
From the resulting p(t), it is an easy task to compute the reference velocity v(t) and acceleration a(t). Erroneous “measurement” â(t), containing scale factor error s and bias term μ with additional white noise, is generated as follows:
(22)â(t)=(1+s)a(t)+N(μ,σ2).
In Figure 2, one realization of â(t) (s=0.05, μ=-0.50 and σ2=1), sampled with a frequency of 50 Hz (N=501), is presented with the reference acceleration a(t).Figure 2
Actual acceleration (thick line) and “measured” acceleration (thin line) as a function of time.Now, suppose that along withâ(t), we know the position and velocity of the object at points t=0 seconds and t=10 seconds. Thus, we have the following problem: find p̂(t), 0≤t≤10 such that
(23)p̂̈(t)=â(t),p̂(0)=0m,p̂̇(0)=0m/s,p̂(10)=10m,p̂̇(10)=0m/s.
The problem here is, of course, that we do not know a(t), but only the erroneous measurement â(t). Thus, ultimately, the goal is to minimize the effects of the measurement error using the given two additional boundary conditions.To exploit method I, we must first generate a suitable prior, which can then be used to enhance the accuracy of the solution. For this, let us compute a polynomial of the lowest order that satisfies the given four conditions at the boundaries and use that as our prior. In this case, a suitable choice for the regularization matrixD would be N×N identity matrix. However, to emphasize the fact that we have no trust in our prior between the two boundaries, we will zero out all but, say, 10 first and last rows of D. Thus, in this example, M=20. Method II follows from the method I by setting λ=0. For method III, we use the error model presented in [1], modeling constant scale factor and bias errors using the velocity of the object at t=0 and t=10. Knowing the form of the generated measurements, this model will yield the exact solution when no noise is applied. The idea here is, however, to test the performance of the method III in case of noisy measurements.


### 4.2. Results

Table1 summarizes the test results, which were run with a total of nine different combinations of error parameters s and μ. Each test was run several times with different realizations of random noise in order to show the average performance of the tested methods. As an overall comment, the tested methods are not particularly sensitive to noise. The accuracy of the method III is, however, dependent on the value of σ2. This was expected, since method III uses only two measurements to determine the two error parameters, which gets more inaccurate as the noise level increases. The value of λ minimizing the position error is presented only for reference, as there is no practical way to find it in real situations. Value of λL, however, can be found for each measurement using the L-curve criterion. During the test, the value of λ minimizing the velocity error was also recorded, but the difference to the value minimizing the position error was not significant.Table 1
Simulation results for various values of parameterss and μ. Terms σI, σII and σIII refer to the standard deviation of the position error (in meters) in case of solution methods I, II, and III. The value of the regularization parameter obtained via the L-curve method is represented as λL and λ represents the value of the regularization parameter minimizing the position error.
sμσ2argminλσI(λ)λσI(λL)λLσIIσIII−0.05−0.511.1374.2001.1374.5015.3101.236−0.050.010.4530.0001.0241.4380.4530.568−0.050.510.1090.1490.9955.6634.0401.4130.00−0.510.8134.0730.8134.2554.7810.7230.000.010.2820.0000.7371.3300.2820.6930.000.510.1010.2210.7295.1854.3531.0150.05−0.510.2863.4070.2863.6113.8230.7570.050.010.1391.2300.2601.5810.6661.6340.050.510.1320.6780.3365.2434.8810.758â(t)=(1+s)a(t)+𝒩(μ,σ2).Based on the results seen in Table1, the L-curve method seems to work reasonably well. Indeed, in some cases it is even able to predict the optimal value of λ (or at least obtain position error levels very close to the minimum). On average, Tikhonov regularization with λL works much (five to ten times) better than the method II. It can also fail, meaning that the resulting error is larger than the one given by method II. This happened only when the optimal value of λ was zero, and the results of method II were optimal. The L-curve method, on the other hand, performed consistently among the test situations. When compared with method III, the difference is not as clear, although on average, Tikhonov regularization does slightly better. As mentioned above, this is due to the large value of σ2. In situations with lower noise and known main error sources, a combination of methods I and III is also a reasonable strategy. This is due to the fact that the error modeling can reduce such components of error (such as bias) with which Tikhonov regularization is not intended to work with.Figures3 and 4 illustrate the performance of methods I and II in terms of position and velocity. The measurement was the one seen in Figure 2 with parameters s=0.05 and μ=-0.5. Value λ=λL was used to compute the regularized solution. The figures also demonstrate the used prior, only first and last 0.2 seconds of which were actually exploited. Clearly, comparing to method II, the accuracy of the proposed method is remarkably better. In terms of the standard deviation, the proposed method is about 13 times more accurate than method II. When comparing to method III, the proposed method is about 2.5 times more accurate, which is also a significant improvement.Figure 3
Actual position (solid line), estimated position computed with method I (dashed line,λ=3.611), estimated position computed with method II (dotted line) and used prior (dash-dotted line) as a function of time.Figure 4
Actual velocity (solid line), estimated velocity computed with method I (dashed line,λ=3.611), estimated velocity computed with method II (dotted line) and used prior (dash-dotted line) as a function of time.In Table2, the computational complexity of the proposed method is demonstrated. The number of samples (N) and constraints (M) was increased by increasing the sampling frequency, keeping the problem otherwise unchanged. The value of λ was kept fixed. As the solution methods were not optimized for maximum performance, the differences in the solution times may not be fully comparable. Especially in case of the LSQR method, default tolerance (relative accuracy of 1e-6) was used. Despite the “large” number of unknowns, the solution times are small for any solution method. Based on the results, one can expect linear time complexity with respect to N+M for similar problems. In the last case with N=128000, the efficiency of the CSNE method drops as the number of iterations increases to seven. This is due to the fact that the conditioning of the system is comparable to N2. Otherwise, the CSNE method seems to provide good efficiency with a reasonably small cost in the accuracy.Table 2
Computing times asN and M increase. All times are average computing times over 100 cycles and include only the solution phase. Tests were performed with Matlab 7.9.0 running on a 32-bit operating system. The used computer has a 2 GHz processor and 2 GB of memory. CSNE stands for the method described in Section 3.2, “∖” for the “backslash” operator, and LSQR for the iterative least-squares problems solution routine implemented in Matlab. “It” indicates the number of iterations made by CSNE and δ/N1/2 indicates the “size” of the last correction step.
CSNE“∖”LSQRNMItT (ms)δ/N1/2T (ms)T (ms)50020116.4e-925100040123.4e-847200080143.1e-78124000160185.5e-6162080003201168.7e-53436160006401344.1e-4766332000128021031.7e-514811764000256022124.0e-43022711280005120711063.1e-4635618The main purpose of this example was to demonstrate the performance of the proposed method in situations where the measurement error is biased (due to error termμ) and not normally distributed (due to error term s). That is, in situations where the presented optimality results are not valid. The results indicate that the proposed method is useful also in these situations, commonly encountered in practice. Moreover, the tests indicate that the proposed method can be very efficient with negligible computing times even for large-scale problems with over 100000 unknowns. This also guarantees that when required, also the value of λ can be determined with reasonably small computational cost.


## 4.1. Setting Up the Example

Let us generate the reference positionp(t), t∈[0,10] s as a fifth-order polynomial satisfying the following six conditions:
(21)p(0)=0m,ṗ(0)=0m/s,p(5)=-10m,ṗ(5)=0m/s,p(10)=10m,ṗ(10)=0m/s.
From the resulting p(t), it is an easy task to compute the reference velocity v(t) and acceleration a(t). Erroneous “measurement” â(t), containing scale factor error s and bias term μ with additional white noise, is generated as follows:
(22)â(t)=(1+s)a(t)+N(μ,σ2).
In Figure 2, one realization of â(t) (s=0.05, μ=-0.50 and σ2=1), sampled with a frequency of 50 Hz (N=501), is presented with the reference acceleration a(t).Figure 2
Actual acceleration (thick line) and “measured” acceleration (thin line) as a function of time.Now, suppose that along withâ(t), we know the position and velocity of the object at points t=0 seconds and t=10 seconds. Thus, we have the following problem: find p̂(t), 0≤t≤10 such that
(23)p̂̈(t)=â(t),p̂(0)=0m,p̂̇(0)=0m/s,p̂(10)=10m,p̂̇(10)=0m/s.
The problem here is, of course, that we do not know a(t), but only the erroneous measurement â(t). Thus, ultimately, the goal is to minimize the effects of the measurement error using the given two additional boundary conditions.To exploit method I, we must first generate a suitable prior, which can then be used to enhance the accuracy of the solution. For this, let us compute a polynomial of the lowest order that satisfies the given four conditions at the boundaries and use that as our prior. In this case, a suitable choice for the regularization matrixD would be N×N identity matrix. However, to emphasize the fact that we have no trust in our prior between the two boundaries, we will zero out all but, say, 10 first and last rows of D. Thus, in this example, M=20. Method II follows from the method I by setting λ=0. For method III, we use the error model presented in [1], modeling constant scale factor and bias errors using the velocity of the object at t=0 and t=10. Knowing the form of the generated measurements, this model will yield the exact solution when no noise is applied. The idea here is, however, to test the performance of the method III in case of noisy measurements.


## 4.2. Results

Table1 summarizes the test results, which were run with a total of nine different combinations of error parameters s and μ. Each test was run several times with different realizations of random noise in order to show the average performance of the tested methods. As an overall comment, the tested methods are not particularly sensitive to noise. The accuracy of the method III is, however, dependent on the value of σ2. This was expected, since method III uses only two measurements to determine the two error parameters, which gets more inaccurate as the noise level increases. The value of λ minimizing the position error is presented only for reference, as there is no practical way to find it in real situations. Value of λL, however, can be found for each measurement using the L-curve criterion. During the test, the value of λ minimizing the velocity error was also recorded, but the difference to the value minimizing the position error was not significant.Table 1
Simulation results for various values of parameterss and μ. Terms σI, σII and σIII refer to the standard deviation of the position error (in meters) in case of solution methods I, II, and III. The value of the regularization parameter obtained via the L-curve method is represented as λL and λ represents the value of the regularization parameter minimizing the position error.
sμσ2argminλσI(λ)λσI(λL)λLσIIσIII−0.05−0.511.1374.2001.1374.5015.3101.236−0.050.010.4530.0001.0241.4380.4530.568−0.050.510.1090.1490.9955.6634.0401.4130.00−0.510.8134.0730.8134.2554.7810.7230.000.010.2820.0000.7371.3300.2820.6930.000.510.1010.2210.7295.1854.3531.0150.05−0.510.2863.4070.2863.6113.8230.7570.050.010.1391.2300.2601.5810.6661.6340.050.510.1320.6780.3365.2434.8810.758â(t)=(1+s)a(t)+𝒩(μ,σ2).Based on the results seen in Table1, the L-curve method seems to work reasonably well. Indeed, in some cases it is even able to predict the optimal value of λ (or at least obtain position error levels very close to the minimum). On average, Tikhonov regularization with λL works much (five to ten times) better than the method II. It can also fail, meaning that the resulting error is larger than the one given by method II. This happened only when the optimal value of λ was zero, and the results of method II were optimal. The L-curve method, on the other hand, performed consistently among the test situations. When compared with method III, the difference is not as clear, although on average, Tikhonov regularization does slightly better. As mentioned above, this is due to the large value of σ2. In situations with lower noise and known main error sources, a combination of methods I and III is also a reasonable strategy. This is due to the fact that the error modeling can reduce such components of error (such as bias) with which Tikhonov regularization is not intended to work with.Figures3 and 4 illustrate the performance of methods I and II in terms of position and velocity. The measurement was the one seen in Figure 2 with parameters s=0.05 and μ=-0.5. Value λ=λL was used to compute the regularized solution. The figures also demonstrate the used prior, only first and last 0.2 seconds of which were actually exploited. Clearly, comparing to method II, the accuracy of the proposed method is remarkably better. In terms of the standard deviation, the proposed method is about 13 times more accurate than method II. When comparing to method III, the proposed method is about 2.5 times more accurate, which is also a significant improvement.Figure 3
Actual position (solid line), estimated position computed with method I (dashed line,λ=3.611), estimated position computed with method II (dotted line) and used prior (dash-dotted line) as a function of time.Figure 4
Actual velocity (solid line), estimated velocity computed with method I (dashed line,λ=3.611), estimated velocity computed with method II (dotted line) and used prior (dash-dotted line) as a function of time.In Table2, the computational complexity of the proposed method is demonstrated. The number of samples (N) and constraints (M) was increased by increasing the sampling frequency, keeping the problem otherwise unchanged. The value of λ was kept fixed. As the solution methods were not optimized for maximum performance, the differences in the solution times may not be fully comparable. Especially in case of the LSQR method, default tolerance (relative accuracy of 1e-6) was used. Despite the “large” number of unknowns, the solution times are small for any solution method. Based on the results, one can expect linear time complexity with respect to N+M for similar problems. In the last case with N=128000, the efficiency of the CSNE method drops as the number of iterations increases to seven. This is due to the fact that the conditioning of the system is comparable to N2. Otherwise, the CSNE method seems to provide good efficiency with a reasonably small cost in the accuracy.Table 2
Computing times asN and M increase. All times are average computing times over 100 cycles and include only the solution phase. Tests were performed with Matlab 7.9.0 running on a 32-bit operating system. The used computer has a 2 GHz processor and 2 GB of memory. CSNE stands for the method described in Section 3.2, “∖” for the “backslash” operator, and LSQR for the iterative least-squares problems solution routine implemented in Matlab. “It” indicates the number of iterations made by CSNE and δ/N1/2 indicates the “size” of the last correction step.
CSNE“∖”LSQRNMItT (ms)δ/N1/2T (ms)T (ms)50020116.4e-925100040123.4e-847200080143.1e-78124000160185.5e-6162080003201168.7e-53436160006401344.1e-4766332000128021031.7e-514811764000256022124.0e-43022711280005120711063.1e-4635618The main purpose of this example was to demonstrate the performance of the proposed method in situations where the measurement error is biased (due to error termμ) and not normally distributed (due to error term s). That is, in situations where the presented optimality results are not valid. The results indicate that the proposed method is useful also in these situations, commonly encountered in practice. Moreover, the tests indicate that the proposed method can be very efficient with negligible computing times even for large-scale problems with over 100000 unknowns. This also guarantees that when required, also the value of λ can be determined with reasonably small computational cost.


## 5. Example 2

In this section, we will demonstrate the use of the regularization method with actual measurement data obtained in a real-world situation. The data was obtained for use with a television program of NHK (Japan Broadcasting Corporation) and presented here with due permission. The primary goal was to measure the accelerations to which the passengers were subjected in a car of a roller coaster (The roller coaster in question is called Insane, located at Gröna Lund, Stockholm, Sweden.) In this example, we present a way to estimate the velocity and trajectory of the car with knowledge about the trajectory of the car. It is important to notice that only the trajectory is known, not its time parametrization.


### 5.1. Measurement Setup

The car moved on a planar trajectory of length≈250 m and rotated freely around its rotation axis (orthogonal to the plane) during the ride. In other words, the problem has three degrees of freedom: one for the rotation and two for the displacements. The trajectory is not known accurately, but roughly estimated using the length and an image of the trajectory. Because of this, the estimated trajectory is known to contain significant errors, which makes it a good example of situations with a rather vague prior.The data was collected using an inertial measurement unit (IMU) consisting of the following sensors:(i)
3×±75∘/sec Silicon Sensing CRS10 [33],(ii)
3×±300∘/sec AD ADXRS300ABG [34],(iii)
3×±3 g VTI SCA610-CC5H1A [35],(iv)
3×±12 g VTI SCA620-CHCV1A [36].The IMU is calibrated as described in [37]. Only the sensors of the lower dynamic range were exploited unless the input exceeded this range. In this case, only the sensors with the higher dynamical range were exploited. The sample rate was 1000 Hz, and the total number of unknown displacements was about 200000 in each measurement. Based on the construction of the roller coaster, the initial and the final angle were known to be identical. This allowed the angle to be computed with high accuracy. Thus, the accelerometers were the major source of error.


### 5.2. Exploitation of the Estimated Trajectory

In this section, the methods used to exploit the estimated trajectory are presented. Notice that only the trajectory is estimated, not the time when the car is at a certain point. It is assumed that the horizontal coordinates of the trajectory are stored to vectorX∈ℝP and the vertical coordinates, respectively, to vector Y∈ℝP. The number of the points (P∈ℕ) is assumed to be high enough and the trajectory data “smooth” enough such that the local tangent of the trajectory can be reliably estimated using two adjacent points of the trajectory.Given a horizontal coordinaterj, j∈[1,N] such that Xi-1≤rj<Xi, i∈[1,P], the corresponding vertical coordinate rN+j satisfies the equation
(24)(Yi-Yi-1)rj-(Xi-Xi-1)rN+j=Xi-1Yi-XiYi-1.
Depending on the trajectory, it is possible that the index i is ambiguous, meaning that there can be more than one vertical coordinate corresponding to a single horizontal coordinate. This is the case also here, as can be verified from Figure 5. Thus, it is also required to choose correct i. It is also possible that no suitable index i is found, meaning that the value of rj is out of range. In such a situation, the trajectory is not exploited for j in question. With these notions, the corresponding elements of the regularization term are
(25)Dj,j=Yi-Yi-1,Dj,N+j=Xi-Xi-1,ej=Xi-1Yi-XiYi-1,∀j∈[1,N].Figure 5
Actual trajectory (solid line), estimated trajectory of run 1 (dashed line), run 2 (dash-dotted line), and run 3 (dotted line). Small squares, circles, and triangles indicate the position of the car with three second increments, respectively, for runs 1, 2, and 3. Regularization parameterλ=0.352 for all three runs.


### 5.3. Solution Process

The problem was solved in two phases. In the first phase, only few points of the trajectory were exploited. These were the points corresponding to the local extrema of the horizontal coordinates of the stored trajectory. The goal of this phase was to produce a reasonable initial guess forr, which could then be used to obtain better results when linearizing the trajectory. For this, valid estimates of the indices j when the car crosses these points were required. In this phase, we used a rather high value λ=100 to minimize the amount of values rj that were out of range in the next phase. After this phase, the measurement vector b was updated by setting b←Arλ.In phase two, the trajectory of the car was exploited throughout the track. This was done by linearizing the estimated trajectory at each time instance in the neighborhood of the horizontal position obtained in the first phase, as explained in Section5.2. Variance of each constraint was set to one, as we had no knowledge of the possible differences in the reliability of these estimates. For the first run of the car, the value of the regularization parameter was obtained using the L-curve criterion. The same value of λ was then used also for the second and third run.


### 5.4. Results

In Figure5, the computed three trajectories are compared to the used prior. In each case, the car starts at point [0,0] leaving to the upright direction. The end point was set to the last local extremum of the horizontal coordinates of the estimated trajectory, because the time instance corresponding to the actual stopping point could not be specified. Notice how the three computed trajectories are almost identical, while they follow the provided prior only approximately. Given that no information about the reliability of different points of the trajectory is provided, this indicates that the L-curve method provides useful estimates of the regularization parameter. Since this happens for all three runs, determining the value of λ in advance, as discussed before, is a plausible technique.The small squares, circles, and triangles seen in Figure5 indicate the position of the car with three second increments, respectively, for runs 1, 2, and 3. As seen in the figure, the distances between these marks change significantly, indicating that the used method “allows” the velocity of the car to change between different runs. For the first ≈50 meters of the trajectory, however, the distance between the marks does not change much. This is due to the fact that during these first meters, the cars were lifted up essentially the same way for each run. After the highest point of the trajectory, the car moved freely along the track “driven” only by gravity and affected by the moment of inertia, determined by the placements of the passengers. As seen in Figure 5, the run represented with the circles, for example, attained significantly higher velocities than the other two runs.


## 5.1. Measurement Setup

The car moved on a planar trajectory of length≈250 m and rotated freely around its rotation axis (orthogonal to the plane) during the ride. In other words, the problem has three degrees of freedom: one for the rotation and two for the displacements. The trajectory is not known accurately, but roughly estimated using the length and an image of the trajectory. Because of this, the estimated trajectory is known to contain significant errors, which makes it a good example of situations with a rather vague prior.The data was collected using an inertial measurement unit (IMU) consisting of the following sensors:(i)
3×±75∘/sec Silicon Sensing CRS10 [33],(ii)
3×±300∘/sec AD ADXRS300ABG [34],(iii)
3×±3 g VTI SCA610-CC5H1A [35],(iv)
3×±12 g VTI SCA620-CHCV1A [36].The IMU is calibrated as described in [37]. Only the sensors of the lower dynamic range were exploited unless the input exceeded this range. In this case, only the sensors with the higher dynamical range were exploited. The sample rate was 1000 Hz, and the total number of unknown displacements was about 200000 in each measurement. Based on the construction of the roller coaster, the initial and the final angle were known to be identical. This allowed the angle to be computed with high accuracy. Thus, the accelerometers were the major source of error.


## 5.2. Exploitation of the Estimated Trajectory

In this section, the methods used to exploit the estimated trajectory are presented. Notice that only the trajectory is estimated, not the time when the car is at a certain point. It is assumed that the horizontal coordinates of the trajectory are stored to vectorX∈ℝP and the vertical coordinates, respectively, to vector Y∈ℝP. The number of the points (P∈ℕ) is assumed to be high enough and the trajectory data “smooth” enough such that the local tangent of the trajectory can be reliably estimated using two adjacent points of the trajectory.Given a horizontal coordinaterj, j∈[1,N] such that Xi-1≤rj<Xi, i∈[1,P], the corresponding vertical coordinate rN+j satisfies the equation
(24)(Yi-Yi-1)rj-(Xi-Xi-1)rN+j=Xi-1Yi-XiYi-1.
Depending on the trajectory, it is possible that the index i is ambiguous, meaning that there can be more than one vertical coordinate corresponding to a single horizontal coordinate. This is the case also here, as can be verified from Figure 5. Thus, it is also required to choose correct i. It is also possible that no suitable index i is found, meaning that the value of rj is out of range. In such a situation, the trajectory is not exploited for j in question. With these notions, the corresponding elements of the regularization term are
(25)Dj,j=Yi-Yi-1,Dj,N+j=Xi-Xi-1,ej=Xi-1Yi-XiYi-1,∀j∈[1,N].Figure 5
Actual trajectory (solid line), estimated trajectory of run 1 (dashed line), run 2 (dash-dotted line), and run 3 (dotted line). Small squares, circles, and triangles indicate the position of the car with three second increments, respectively, for runs 1, 2, and 3. Regularization parameterλ=0.352 for all three runs.


## 5.3. Solution Process

The problem was solved in two phases. In the first phase, only few points of the trajectory were exploited. These were the points corresponding to the local extrema of the horizontal coordinates of the stored trajectory. The goal of this phase was to produce a reasonable initial guess forr, which could then be used to obtain better results when linearizing the trajectory. For this, valid estimates of the indices j when the car crosses these points were required. In this phase, we used a rather high value λ=100 to minimize the amount of values rj that were out of range in the next phase. After this phase, the measurement vector b was updated by setting b←Arλ.In phase two, the trajectory of the car was exploited throughout the track. This was done by linearizing the estimated trajectory at each time instance in the neighborhood of the horizontal position obtained in the first phase, as explained in Section5.2. Variance of each constraint was set to one, as we had no knowledge of the possible differences in the reliability of these estimates. For the first run of the car, the value of the regularization parameter was obtained using the L-curve criterion. The same value of λ was then used also for the second and third run.


## 5.4. Results

In Figure5, the computed three trajectories are compared to the used prior. In each case, the car starts at point [0,0] leaving to the upright direction. The end point was set to the last local extremum of the horizontal coordinates of the estimated trajectory, because the time instance corresponding to the actual stopping point could not be specified. Notice how the three computed trajectories are almost identical, while they follow the provided prior only approximately. Given that no information about the reliability of different points of the trajectory is provided, this indicates that the L-curve method provides useful estimates of the regularization parameter. Since this happens for all three runs, determining the value of λ in advance, as discussed before, is a plausible technique.The small squares, circles, and triangles seen in Figure5 indicate the position of the car with three second increments, respectively, for runs 1, 2, and 3. As seen in the figure, the distances between these marks change significantly, indicating that the used method “allows” the velocity of the car to change between different runs. For the first ≈50 meters of the trajectory, however, the distance between the marks does not change much. This is due to the fact that during these first meters, the cars were lifted up essentially the same way for each run. After the highest point of the trajectory, the car moved freely along the track “driven” only by gravity and affected by the moment of inertia, determined by the placements of the passengers. As seen in Figure 5, the run represented with the circles, for example, attained significantly higher velocities than the other two runs.


## 6. Conclusion

A BVP formulation of inertial navigation problems is further investigated using [1] as a starting point. It is suggested that the possible additional information can be taken into account by exploiting Tikhonov regularization, a basic tool of inverse problems. In addition to typical additional information, such as a momentary position or velocity, it allows one to exploit more general forms of information. These include, but are not limited to, average velocity over longer period of time, trajectory of the object, and possible symmetry in the computed result. In the provided examples, significant accuracy improvements—up to an order of magnitude—over the basic FEM solution without any additional information are obtained.It is also demonstrated that the proposed method can be viewed as a Bayesian estimator, yielding the maximum a posteriori likelihood estimator in case of unbiased and uncorrelated measurement errors. In addition to this, the provided examples show that the obtained accuracy improvement is significant, even in cases where these assumptions are not met.A method for choosing the value of the regularization parameter is provided and demonstrated to work in a real-world example. This method, called the L-curve criterion, does not require any prior knowledge of the measurement errors. Thus, it can be used in many real-world situations, where measurement error is always present, but seldom reliably characterized. For performance-critical situations, it is possible to determine a suitable value for the regularization parameter in advance and use this value to obtain good results.In some applications, it is possible and advantageous to combine the method based on sensor error modeling [1] with the method proposed herein. Sensor error modeling is used first to eliminate the modeled deterministic measurement errors, and Tikhonov regularization is then used to minimize the effects of the remaining stochastic components of the measurement error.



---

*Source: 450269-2011-04-14.xml*