Abstract
The term inertial navigation is often automatically associated with the term initial value problem. However, there are many applications where it is possible to end up with a boundary value problem (BVP) as well. We show that in case of a BVP, the finite element method that incorporates boundary conditions can be efficiently used to compute position and velocity estimates not prone to accumulation of errors. For further accuracy enhancements, a method of combining inertial measurements with additional constraints is proposed. This way, we can model sensor errors, known to limit the accuracy of the system. The capabilities of the proposed methods are demonstrated with real-life examples.
1. Introduction
Typically, inertial measurements are made to have estimates of current position and velocity in real time. The set of equations used to compute the position and velocity estimates out of the actual measurements depends greatly on the application in hand. Equations for a general navigation application are presented, for example, in [1] and [2–5]. In this study, however, we shall concentrate on the growing market of consumer applications employing inertial sensors within a suitable price (and quality) range. Thus, we can simplify the equations needed to compute the position and velocity estimates. This is so, because in this case the errors are more likely to be determined by the limited accuracy of the sensors rather than the accuracy of the used equations.
In terms of the simplified equations, we basically need to solve the following problem: given , find such that satisfies In (1), represents the second time derivative, is the position of the object in a suitable coordinate frame, represents the acceleration of the object in the same coordinate frame, and is time. The velocity of the object (), given in the same coordinate frame as and , is denoted as . Notice that the form (1) follows from the more general navigation situation by neglecting the rotation and “curvature” of the Earth.
Now, from the theory of ordinary differential equations (ODEs), we know that we need exactly two independent constraints to unambiguously solve a 2nd order ODE (1). Particularly, if these two independent constraints are both given at time , we have an initial value problem (IVP) [6]: given , , and , find such that hold. Problems that are not IVPs based on the above definition are called boundary value problems (BVPs). A special case of a BVP, convenient to our purposes, is stated as follows: given , one of the constraints
and one of the constraints
find for such that it satisfies We know for a fact that a problem of the form (5) has a unique solution whenever the position is fixed at least at one boundary: This knowledge comes from the fact that (7) is a one-dimensional case of a certain class of partial differential equations called Poisson problems, properties of which are well known [7].
Notice that there is a natural reason why problems (2) and (5) are distinguished. Namely, the solution methods for IVPs and BVPs differ substantially from each other [6, 8]. This is also where this paper differs from numerous research articles considering inertial navigation: previously, the problems have been given in the form (2), whereas we consider problems of the form (5).
The key assumption of our approach is the following: inertial measurements related to a certain time period, including a set of boundary values and possibly some additional constraints, are all available when processed. The length of the time period (i.e., domain) in (5) can be anything from fractions of a second to some minutes. There are different ways we may end up with a BVP of the form (5): firstly, the underlying problem may naturally be a BVP. Secondly, we may have an IVP, which we can pose as a BVP. This, of course, requires that we also know something about the result at the end of the interesting event and that we can afford to wait for the results until the end of the event. In the first case, we generally do not have any additional constraints we could use, but in the second case we end up with a BVP and at least one additional constraint. We will discuss examples of both cases in the next section.
The case where we only know the velocity at both ends is, however, special and deserves some attention. According to the discussion above, we do not have a unique solution for this kind of a BVP although it fits the definition of our model BVP (5). It is neither a valid IVP according to (2). As it turns out, it is possible to solve also this without loss of generality. This is based on the fact that we can always fix the position at one boundary without changing the “shape” of the solution. Then, we will come up with a well-defined BVP with an additional constraint.
The overall scope of this paper is to show how to treat inertial navigation problems that are naturally (or knowingly) posed as a BVP of the form (5). Attitude computations are not considered, and where necessary, attitude is assumed to be available. The form of problem (5) allows us to consider it as a set of three one-dimensional (1D) problems, rather than one three-dimensional (3D) problem. Notice that two boundary values per a dimension are required to obtain a unique solution. On the other hand, there is no need for the boundary values for different dimensions to be of the same type.
This paper is organized as follows: in Section 2, preliminaries are discussed. In Sections 3 and 4, we will show how to exploit 1D finite element method (FEM) to solve the underlying BVP with various possible choices of boundary conditions. At this stage, we assume that no additional constraints are given and that the measurements are exact. In Section 5, we face the reality with faulty measurements and exploit linear additional constraints to enhance the accuracy of the results. The underlying BVP is treated as exact, but the measurements are corrected using a linear sensor error model. As a result, we get two systems of linear equations: an exact one for the BVP and possibly an overdetermined one for the parameters of the sensor error model. We will solve these together to yield the corrected results. Finally, two real-life examples are discussed in Sections 6 and 7 before the conclusions in Section 8.
2. Preliminaries
Let us first motivate the chosen approach by means of examples: an application suitable to our approach is ski jumping, which has been the prime motivation of this study [9]. As an inertial navigation problem, it includes the use of consumer grade sensors with knowledge of the boundary values (in this case, position at both ends of the event). See Section 7 for further details.
It turns out that especially sports applications tend to have properties that are well suited to the considered approach: the included actions are often periodic, in such a way that the certain short-term action (e.g., a single step) repeats several times or some longer-term action a few times (e.g., a single lap or a single jump as discussed earlier) during a certain event. In these kinds of applications, it is natural to encounter problems of the form (5) rather than (2): for example, in long jump (considering only the jump part), at time (“take off”), the velocity of the shoe is known but position is not and at time (“landing”), the position of the shoe when it hits the surface of the sand can be accurately measured but the velocity is unknown.
For a more general view, even a GPS-assisted inertial navigation system can be considered as a series of separate navigation periods with given boundary values rather than a single event with additional constraints given at certain time instances. This is an example of an IVP, which can be posed as a BVP.
2.1. Some Remarks
There are two main classes of numerical methods for BVP's. One class includes so-called shooting methods and the other class methods of weighted residuals such as FEM [8, 10]. We concentrate on the latter because of its property to minimize an error norm over the whole integration interval rather than minimizing only the local error [7]. Another tempting property is that it concerns the position directly, giving us a possibility to more easily handle various types of additional constraints we will encounter later on.
In many applications, inertial measurements are not the only source of information. In practice, however, the number or the type of these additional constraints—combined with the different kinds of a solution method—does not suggest the use of the traditional filtering (see Section 5 for details). For these situations, we will introduce a computationally cheap and easily exploitable method to enhance the accuracy of the position and velocity estimates. The proposed method is based on sensor error modeling and is characterized by the following assumptions. (i)Sensor errors are modeled as constant errors. While the behavior of a certain sensor error is in real life a stochastic process, one is usually able to fairly model it at least momentarily as a constant error. A suitable mathematical tool to characterize this is the Allan variance [11].(ii)Considering consumer grade sensors, causes of the most significant errors are usually known (e.g., bias and scale factor error, both changing from turn-on to turn-on [2]).(iii)In particular, the sensor noise is not modeled. This is a conscious modeling decision to prevent unnecessary “smoothing” of data.(iv)Additional constraints are treated as “exact”. That is, the overall error in the additional constraints is assumed to be smaller than the error caused by the simplification of the navigation equations.
When additional constraints are available, problems resembling the ones considered here have previously been resolved using the means of fixed interval smoothing (or “Kalman smoother”, if the type of the filter is fixed) [12–15]. Considering inertial navigation, the most used methods of solving the problems are the two-filter smoother [16] and Rauch-Tung-Striebel smoother [17], used for example in [15, 18–20]. In both cases, the problem itself is posed as an IVP and the basic idea 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 while combining these two results to yield the corrected result in phase two. Between the proposed method and the fixed interval smoothing method, the fundamental difference is that in the proposed method the dynamics model is based on the BVP formulation and in the previous approaches, on the IVP formulation with additional smoothing.
Comparing the fixed interval smoothing technique to the proposed method, in addition to the previously mentioned points, the most significant differences are as follows.(i)As fixed interval smoothing is run in both directions, the filter needs two process models, which can be problematic [21]. As the proposed method is based on the BVP formulation, it does not make a distinction between forward and backward directions.(ii)In the filtering approach, each additional constraint is assumed to be attached to a single time instance [2], while the proposed method does not make such a restriction (see Section 5 for details).(iii)Fixed interval smoothing is a two-phase method requiring numerous computations per time step [2, 15], whereas the proposed approach is a single-phase method with only few computations per an unknown.
Due to the significant differences in these two approaches, we will in this context concentrate on the proposed method. Obviously, there are situations where the two methods could both be used. Comparison of the methods in such a situation is interesting, although not addressed in this paper.
Finally, recall that problem (5) can be considered as a set of three 1D problems. Thus, let us focus on the 1D case for a while. Details of the more prevalent 3D case are considered later on. Notice that because of this, the symbols will be changed a bit: whereas and so on. In the following treatment, it is assumed that the accelerometer samples represent an instantaneous value of the specific force. In other words, it is assumed that the sensor output is not processed in any way before the “navigation computer.”
3. Solution of a BVP
The main goal of this section is to form a linear system of equations of the form for the position estimates . These are now expressed as a vector containing the position at each discrete time instant (referred to as ), where the vector is a function of . In the following treatment, the total number of the samples is , is the value of time instant (), and .
Now, let us rephrase problem (5) as a variational equation where is a Hilbert space [7]. Second derivative of can be eliminated by integrating the left-hand side of (6) by parts, which yields where the notation “” stands for substitution. By evaluating the substitution term and rearranging (7), we get
While (8) is otherwise in a convenient form for our purposes, it is not well suited for practical computations, because is an infinite-dimensional space. Thus, let us approximate with a finite-dimensional space spanned by piecewise affine basis functions (“affine function = linear function + a constant”) often referred to as the “hat” functions. Functions (9) for few values of are shown in Figure 1. Also, let us use the same basis functions to discretize and . This choice is often referred to as the Galerkin method [7]. Note that it is also possible to choose basis functions different from the lowest order approximation used here, when considered necessary.
Given the basis functions , notice that the position can be approximated as a piecewise affine function From (8), we see that the derivatives of the basis functions (9) are also needed. It holds that From Figure 2, one can verify that function is a piecewise constant function, discontinuous at points , , and . From (8) we see that we need to integrate a similar term with discontinuities at the nodes over the domain. In other words, these discontinuities do not matter, which is a well-known fact from integral theory.
In total, we are now in the position to discretize equation (8), which yields
Now that we have discretized the problem, we are getting closer to the equation stated as our goal. In fact, (12) is a system of linear equations. Thus, let us start by assembling the matrix . Knowing that the index in (12) refers to a certain column and to a certain row, the elements of are given as which are easy to compute by substitution of (11) into (13). In practice, is going to have only few nonzero elements all of them at the diagonal, subdiagonal, and superdiagonal (assuming the “obvious” indexing). For equally spaced nodes with step size , for example, we have elements at the diagonal and at the sub-and superdiagonal.
Our next task is to compute the vector . As seen from (12), the task is to integrate term over the interval . To do this, we fit a piecewise affine function to between every node as seen in Figure 3 with dashed line, which yields for every . For nodes and , we get respectively.
Finally, let us consider how to apply the different types of boundary values into (12). At first, notice that terms and seen in (12) will be nonzero (evaluating to value one) only for the values and , respectively. If or of (5) is given, the respective boundary condition is called a Neumann boundary condition [7]. These can be applied directly by adding the given values into .
The other possibility for the boundary values is to have or fixed thus having a Dirichlet boundary condition [7]. As the value at the corresponding boundary is already known, it does not need to be solved. Thus, we can move all terms depending on it to reducing the number of unknown terms by one. For the reduced system, the corresponding term of the last two terms of (12) will be zero, as mentioned above. Recall that it is also possible to have a Dirichlet condition on one boundary and a Neumann condition on the other boundary.
We have now means to assembly an equation of the form for the (1D) position of the object. Depending on the type of the applied boundary conditions, the number of unknowns is equal to or . Matrix is known to be symmetric and positive definite [22]. With the chosen basis functions, is also a tridiagonal matrix. In practice, these properties guarantee that (16) can be solved with linear time complexity [8]. In other words, when doubling the amount of unknowns , the time needed to solve the system is also doubled (approximately), which is certainly not true for a general system of linear equations.
From the position data , it is now a straightforward task to compute the velocity using a suitable numerical differentiation formula. Since the position data is relatively smooth due to the “double integration” process, we have not experienced any problems in computing the derivative with an adequate accuracy.
4. Generalization to 3D
In this section, we will generalize the method described in the previous section to the 3D case. For this, we introduce a coordinate transformation matrix used to transform the specific force measurements into the accelerations represented in a suitable navigation frame as follows: An element-wise representation of is
In this paper, we will assume that an estimate of for each time instant is available. Note that depends only on the attitude, which is independent of the position and velocity, when the assumptions considered in the first section are valid [2]. Furthermore, let vector be the acceleration due to the gravity represented in the navigation frame. With these notions, the specific force measurements made by the accelerometers can be “converted” into accelerations as where the time dependencies have not been explicitly stated.
Now, following from (19) and the right-hand side of (12), we have an equation
for the th component of vector . Thus, the linear equation for the 3D position is As discussed in the previous section, matrix is a tridiagonal matrix. Then, also the system matrix of (21) is not only tridiagonal, but a block diagonal matrix. For instance, if boundary conditions are fixed in the same way for all dimensions, it follows that holds.
With the equations derived in this section, it is possible to solve the 3D BVP using the presented FEM method. The solution of this more general problem can also be found with linear time complexity, as in the 1D case.
5. Using a Number of Additional Constraints to Model Sensor Errors
So far we have constructed a method to compute position and velocity data with certain boundary conditions. We will now propose a way to exploit a number of additional constraints concerning the velocity or the position information to estimate sensor errors. For this, let us now precisely define the term “additional constraint”: an additional constraint is a linear equation that bounds the position or the velocity of the object at an arbitrary number of time instances . At simplest, the previous definition could simply mean that is fixed. On the other hand, a less intuitive equation is also a valid constraint. This way, we can for example exploit constraints like for any and without saying anything about the absolute position at these points. It could be hard to exploit these kind of constraints properly in traditional filtering problems.
In this section, we will use constraints systematically to enhance accuracy. To make the concepts presented in this section clear, let us first present the equations for the 1D case.
5.1. Treating Additional Constraints
Let us first assume that holds. That is, given a Dirichlet boundary condition, we add an auxiliary equation to the system instead of removing the corresponding boundary value from the system. This is a necessary procedure when exploiting additional constraints.
In general, all linear constraints can be represented in the form where and hold, where is the number of the constraint equations. For a practical example closely related to the example 1, consider that (5) is solved with Dirichlet boundary values. If the Neumann boundary values are also known and the samples are equally spaced, one can construct (22) asMatrix is formed using a three-point differentiation method based on polynomial interpolation [10] to determine Neumann boundary values from the displacement data.
These constraints could be exploited just by computing the linear least squares problem constructed by adding these additional equations to (16). Usually, however, it is favorable to use the additional constraints to model sensor errors, at least when one has any knowledge of the types of the errors included in the measurements. For more information on the assumptions related to the sensor error modeling, recall Section 2.1. In this text, we will present a linear sensor error model.
5.2. Sensor Error Model
In typical situations, bias offset (i.e., the sensor shows nonzero output when no forces are acting upon it) and scaling factor are the two terms which have to beknown very accurately in order to have any realistic position or velocity estimate as a solution. Because of the run-to-run variations of these errors, they cannot be assumed to be constant between two separate events. Thus, they should be treated as unknowns. Other typical errors are caused by misalignment of the axes, changing temperature and drifting bias.
As a practical example used in the example 1 later in Section 6, let us model the sensor errors as follows: where is the corrected specific force measurement, is some constant scaling factor, and is some constant bias term correcting the erroneous measurement. By replacing the measured treated in the previous section with the corrected acceleration , it is easy to form an equation of the form for vector seen in (16). Matrix is in this case an matrix, whose elements are formed by computing the integrals and from (12). Vector is simply .
Let us now replace the right-hand side of (16) with where depends only on the given boundary conditions (which are treated as exact) and corresponding boundary values. This distinction is necessary, since one should not modify the given boundary values by applying error modeling on them.
In general, a problem of linear constraints used to model linear sensor errors can be stated as where , hold, and is the number of modeled sensor error terms.
Now, since is known to be invertible, one gets an equation for . Note that the matrix has dimension , that is, it is typically a very small matrix compared to . With known, it is easy to solve for .
In the case where (more constraints than model parameters), one can also take reliability of different measurements into account by solving a weighted least squares problem (WLS) [22]. In general, (29) has a unique solution only when holds and the row rank of is full. Typically, making sure that and that the row rank of is at least , (29) has either a unique or a least squares solution for . In each case, the upper equation of (28) is treated as an exact equation.
5.3. Additional Constraints in 3D
Let us now consider the use of additional constraints in a 3D case. Using the notions form the previous section and (22) as an example, constraints can be represented as
In addition to the sensor errors discussed in the 1D example, it is now also possible to model errors like attitude errors, unknown value of the acceleration due to the gravity, and cross-correlation of different sensors. A particularly useful method is to treat the acceleration due to the gravity as an unknown three-dimensional vector, which is then subtracted from the specific force measurements given in the global coordinates. This reduces the systems sensitivity to initial attitude errors.
As an example, let us now derive the equations for sensor error of (25) for each axis in addition to the unknown acceleration due to the gravity discussed above. Thus, we have a total of unknown model parameters. As one could expect, we must take the effects of the coordinate transformation matrix into account when deriving the necessary equations. By plugging (25) into (21), we have The several integrals in (31) are scalars for each , easily evaluated with (14) and (15), since no unknown terms appear inside the integrals. Let us now gather the results into vectors for all . With these notions, we get, for example where only the relevant indices are shown. Thus, we get size For the nine unknown model parameters we need at least nine constraints. This can be covered, for example, with the knowledge of the velocity of the object at three different points. In total, we have equations identical to (28), with dimension replaced by .
6. Example 1
To demonstrate the use of the proposed method, an example using readily available consumer grade accelerometers [23] was created. An accelerometer was mounted on a horizontally rotating rate table, whose angular velocity (rotation rate) can be controlled. The accelerometer was mounted in such a way that its measurement direction was approximately the same as the direction of the tangential acceleration caused by angular acceleration of the rate table, as seen in Figure 4.
The aim of this example was not to get position and velocity as accurately as possible, but to compare different solution methods in a situation where one needs to get reasonable position and velocity estimates regardless of the fact that the measurement contains significant errors. The angular velocity of the rate table was set to follow function illustrated in the Figure 5 a certain number of times. With this kind of a setup, the true acceleration, velocity, and displacement of the mounted sensor were known up to the accuracy of the motor rotating the rate table. The errors caused by the rate table were observed using the angular rate output of the motor and found to be negligible compared to other error sources.
In first test, the rate table was set to repeat exactly the function represented in Figure 5 ten times, leading to seven full revolutions (or meters) in seconds. In the other test, the function of the same form was scaled in such a way that repeats lead to total of full revolutions (or meters) in seconds. The sampling frequency of the sensor was set to Hz and an accurate ( bit) analog to digital converter was used. Accelerations were measured using a consumer grade g accelerometer [23].
The raw accelerometer data was mapped to accelerations with the scale factor given by the manufacturer. From this acceleration data, position and velocity were computed by a number of different methods:(i)“Traditional” IVP (double integration with trapezoid rule),(ii)FEM with Dirichlet boundary conditions (BCs), (iii)FEM with Dirichlet BCs combined with additional Neumann BCs to supply the error model (25).
In methods I and II, the bias error of the sensor was estimated by averaging the output while the sensor was at rest. This was done in order to make method I (and to some extent, method II) comparable to the method III by reduction of the large bias error. This is rarely possible in general and only method III can be used to reliably detect any remaining bias (example 2 in Section 7 is an example of this case). As a reference, the ideal (ideal driving motor) and the measured accelerations (accelerometer bias removed) of the first spin are plotted in Figure 6.
Table 1 shows the main results of the tests. In each test, the computed results were compared to the known value in three separate points by computing the difference between the computed and the real value. Point was located at , at , and at .
As seen in Table 1, method I seems to increase the error with increasing time, as expected. Method II on the contrary, thanks to the basic property of the variational technique, does not increase the error but distributes it over the whole time period. The difference between these two methods is clearly seen in the velocity and position errors of the longer test (test 2), where method II gives much better estimates than method I. In each test, method III clearly outperforms methods I and II, which is expected due to the provided two additional constraints.
Figures 7 and 8 demonstrate the differences between velocity and position estimates given by methods I and II during test 1. Estimates given by method III coincide with the reference plots. Figure 7 shows only the last spin of the test 1 for better view of the differences.
7. Example 2
This example considers the computation of the velocity and position of a ski jumper during a single jump. As compared to the previous example, this is a more realistic and general inertial navigation problem with six degrees of freedom. Computation of the attitude of the object is based on the data given by three standard consumer grade gyroscopes [24]. The position and velocity were then measured with three standard consumer grade accelerometers [23] and computed with the proposed method with a sensor error model similar to the one presented in Section 5.3. The needed additional constraints contained information about(i)the location of the jumper at five points evenly spread on the inrun hill (in Figure 9, the part of thick black line with negative - and positive -coordinates)(ii)the trajectory of the jumper after the landing, which should coincide with the linearization of the landing hill (in Figure 9, the part of thick black line with -coordinates greater than m).
In Figure 9, two-dimensional trajectories of two jumpers are plotted along with the known profile of the hill. At first, the trajectories follow the profile of the hill until the jumpers take off and eventually land at some point of the landing hill. Drawn circles at both ends of the trajectories demonstrate the start and the end of the navigation period and the given Dirichlet boundary conditions. Drawn squares represent the landing points, in this case quite different for the two events, agreeing well with the recorded jump lengths. Notice that the trajectories are computed using two distinct measurement systems, each containing unique error sources.
Figure 10 shows that the vertical component of the velocity of the same two jumps is plotted as a function of horizontal displacement. Notice how the absolute values of the vertical velocity substantially differ while the small changes in the velocity are practically identical. One might first consider the small changes as typical stochastic errors caused by the inertial navigation system, especially when dealing with low performance sensors.
Given that two independent measurement systems show the same variations in the velocity at the same locations, it is evident that there is actually only a negligible amount of stochastic errors present. Instead, the small variations are caused by deterministic sources, namely, in this particular application the uneven inrun hill.
Unfortunately, the estimates cannot be compared with a reference trajectory, because such data are not available. Thus, we cannot give the exact amount of error present in the position and velocity estimates. We do however claim that the achieved accuracy is something one does not typically expect from consumer grade inertial sensors.
8. Conclusion
The work was motivated by applications, where it is natural to encounter BVPs instead of IVPs. In many cases, it is also possible to formulate an IVP as a BVP, given that the results are not required in real time.
Finite element method is utilized to solve inertial navigation problems formulated as BVPs. As a result, we get a linear system of equations for the position estimates, whose solution can be found with linear time complexity. It is demonstrated that solving a BVP rather than an “equivalent” IVP gives more accurate results.
For further accuracy enhancements, an efficient way of combining inertial measurements with possible additional constraints is created. This gives us a possibility to model constant sensor errors, known to limit the achievable accuracy of the system. While the error model significantly enhances the accuracy of the system, it is kept computationally simple and easily adoptable.
In practice, the accuracy improvements allow us to exploit inertial sensors of certain performance level in more challenging applications. For this, it is necessary to see that the concept of inertial navigation does not invariably imply an IVP, but a BVP as well. Then, the use of FEM will provide an efficient way to compute position and velocity estimates not prone to the accumulation of errors.
In larger scale, the current paper serves as an introduction to the idea of formulating inertial navigation problems as BVPs. As a consequence, further studies are needed to address problems to which the presented tools do not provide an obvious solution. These include, for example, stochastic errors, reliability of the possible additional constraints (as compared to the accuracy of the IMU), and coupling of position and attitude errors.