User:Charlie031541

The Most Proable State Vector Estimator is based upon the concept of determining the propabale value for the solution to a dynamic estimation problem. A dynamic estimation is definned by its time dependent differential equation problem state vector. new estimatorDynamic Estimation is the branch of mathematics used for estimating values for a set of time dependent variables, at a specific time, given a time dependent differential equation for these variables. This differential equation can contain parameters whose values are either represented by apriori known time Gaussian Random Variables and/or time independent parameters whose values can also be estimated. Also available are functions that map some and/or all of these time dependent variables, at known times, to values that are represented by known Gaussian Random Variables.

  A New Solution to the Tightly Coupled GPS Aided Navigation  Estimation Problem 

Abstract

This paper introduces a more general dynamic state vector estimation methodology, referred to as the Multi Cycle Estimator (MCE), which is based on the principle of determining the value of the state vector that maximizes a derived closed form expression for the probability density of the state vector. A basic advantage of the MCE is that, unlike Kalman filter theory, the incremental state vector update can be expressed as a linear combination of sensor data corresponding to different time tags. Thus, at a at a minimum, the MCE allows for a more processing efficient estimator since only one matrix inversion is required for the processing of any arbitrary number of sensor data cycles. This means that for the tightly coupled GPS /INS integrated estimation problem all the GPS pseudo-range and Doppler frequency data can be used when the position, velocity and orientation estimation updates are performed at one Hertz rate. The MCE also lead to a step by step deductive mathematical derivation of the Extended Kalman filter (EKF) equations that results in a more processing and numerical efficient way of propagating the covariance matrix and a closed form derived expression for the Q-matrix. This suggests that the Extended Kalman filter equations can be used to optimally solve the full state formulated strapdown navigation estimation problems. The term optimal in the MCE methodology means when performing a Monte-Carlo simulation, the tabulated root-mean-squared estimation errors will be statistically consistent with the Cramer-Rao Lower Bound matrix [1]. Evidence of this optimality for a simulated GPS aided 34 state vector strapdown navigation estimation problem is presented. The state vector consists of position, velocity and orientation (nine states) -- IMU biases, scale factors and alignment angles (24 states) -- time delay of the receiver.

Introduction

Assume you were given the task to write a paper on real time dynamic estimation that is intended for engineering graduate students who are totally new to the subject. The goal of the paper is to explain the concepts and equations so clearly that the students will not only have intuitive understand its concepts but also be able to use the equations to solve real-world estimation problems without asking any questions or receiving any outside help. Some of you might have the technical writing skills to complete such a task, however I do not. Since the basic concepts of the proposed estimation methodology is significantly different than the standard Kalman filter, the subject presented in this paper is, for all practical purposes, totally new. Thus, the purpose of this paper is to introduce real-time dynamic estimation a methodology that has the potential to provide the engineering community with a mathematical tool that can used to better solve certain estimation problems, such as the GPS aided navigation estimation problem. It is not purpose of this paper to explain how to use this tool.

One of the basic differences between the MCE formulation and the standard Extendend Kalman filter formulation is it is assumed that physical process that operate on the system are deterministic in the sense that it is theoretically possible to measure the time derivatives caused by these processes to with any desired accuracy. However it is assumed that the real-world sensors used to measure the derivatives produce values that are assumed to have significant uncertainties. In the MCE formulation these values are treated as Gaussian Random Variables. This using formulation it is now possible to

and thus these values are treated as .   these it might ere is a closed form mathematical expression for differential equation that describes the time evolution of the dynamic components of the state vector.  are represented by parameter that contain  derivatives of the dynamic components of the state vector are physically deterministic, however they are modeled by expression  parameters of the mathematical expressions they there equations which are Its derivation of the EKF equations proves that, from a theoretical standpoint, the EKF optimally solves  time-uncorrelated (uncertainties in the GRV parameters of the differential and senor measurement equations are not correlated in time) single cycle recursive estimation problems whenever the state vector’s PDF can be continuously approximated by a Gaussian.  Therefore, even though the EKF cannot optimally solve all types of nonlinear The Multi Cycle Estimator (MCE) consists of a new formulation to the dynamic state vector estimation problem methodology which contains new concepts that make it possible to derive, for the first time, a theoretically accurate closed form expression for the state vectors PDF, even in the presence of so-called nondeterministic dynamics.  estimation problem, there is a significant class of problems that it can optimally solve.  As demonstrated in this paper, when the uncertainty in the external sensor data is modeled as Gaussian white noise and the apriori state vector information is precise enough, the full state formulated strapdown navigation problem is an example of estimation problem that can be optimally solved by a properly implemented EKF.  However when the sensor data occurs at a high rate     

Unlike the traditionally used error model formulation, the full state formulation utilizes inertial acceleration and inertial angular-velocity dynamic models and has, as inputs to its processing, the inertial measurement unit’s (IMU) sensed inertial acceleration and angular-velocity. Excluding the white noise errors, the real-world accuracy of the strapdown navigation dynamic model completely depends upon the real-world accuracy of the employed functional mapping of the IMU-sensed inertial acceleration and angular-velocity, to, the real-world inertial acceleration and angular-velocity expressed in the body axes. The to-be-estimated parameters of this functional mapping are represented by the static components of the state vector. According to the derivation, each unique sensed-to body inertial functional mapping implies a well defined unique nonlinear transition matrix. And, according to the derivation, each assumed set of IMU white noise statistics implies a well defined unique Q-matrix. In addition to the 9 dynamic parameters, the simulated state vector contained 24 static parameters representing gyro and accelerometer biases, scale factors and misalignment angles. According to the simulation results the implementation of the derived unique expression for the nonlinear transition matrix and the derived unique expression for the Q-matrix resulted in an optimal and robust 33 state vector estimator.

This paper makes the surprising claim that the standard formulation of the real-time estimation problem that has been used for the past four decades, because it does include a methodology for deriving an accurate expression for the PDF of the state vector, has been a roadblock in the development of optimal nonlinear estimation techniques. To present evidence of this two quotes are taken from reference [3]. Although first published in 1974, even today it is arguably the most referenced text book in this field. Quoting from the forward section and the back cover of that book “Heuristic, rather than theoretically elegant, arguments are used extensively, with emphasis on physical insights and key questions of practical importance”. In computer science a heuristic is a technique designed to solve a problem that ignores whether the solution can be proven to be correct. In other words, it is Gelb’s way of saying that much of his book is not based on mathematical proofs or derivations. It is the opinion of this paper that, when using a set of equations to solve a problem it is easier to obtain a physical insight when one has an intuitive understanding of a mathematical derivation of the equations. Quoting from the introduction section to Chapter 6 of the same text book “For several reasons, the problem of filtering and smoothing for nonlinear systems (almost all real-world systems are nonlinear) is considerably more difficult and admits a wider variety of solutions than does the linear problem”. In this paper’s formulation of the single cycle recursive estimation problem only one solution is admitted, i.e. the same solution admitted by the century old Maximum Likelihood Estimator (MLE) [4]. In the MLE, as its name suggests, the solution is taken to be the value for the state vector that maximizes a derived expression for the state vector’s PDF and, according to this paper, its basic concepts should be also employed for the solution to real-time dynamic estimation problem, even when the dynamics are nondeterministic. In this paper, nondeterministic dynamics implies the system’s differential equation contains parameters that are treated as known Gaussian Random Variables (GRVs).

At the Institute of Navigation conference in January 2004, Shapiro presented a paper [5] which described the derivation of the general expression for the state vector’s PDF as a function of the apriori information and all the past and present sensor data. The derivation included estimation problems that have nondeterministic dynamics. In addition to describing how to determine its MLE estimate, that paper discussed a basic theoretical flaw common to all single cycle recursive estimators -- they do not allow for the simultaneous processing of sensor data from more than one data cycle when updating the state vector. This flaw comes into play when the apriori state vector information is inadequate to allow for the state vector’s PDF to be continually approximated by a Gaussian. A an approximate Gaussian PDF means its log-likelihood function (LLF) can be accurately approximated by a quadratic expression in components of the state vector. Many tracking estimation problems falls into this category as the apriori information for the state vector are often non-existent. To use an analogy, applying the EKF to an estimation problem which results in a LLF that is not quadratic, is like applying the quadratic equation to a third order polynomial equation and ignoring the cubed term. The suggested solution to these types of estimation problems is to apply an iterative batch processing MLE optimization type methodology to the derived expression for the LLF until there is sufficient enough input data so that, as according to the Central Limit Theorem, the LLF can be approximated by a quadratic.

However if the PDF can, from the start, be accurately approximated by a Gaussian then a properly implemented EKF should be able to produce nearly optimal estimation results. According to simulation studies performed after January 2004, the state vector’s PDF for the strapdown navigation estimation problem can, due to its sufficiently precise apriori information, be continually approximated by a Gaussian even when employing the nonlinear full state formulation. However, there is an implementation flaw that is promoted in many text books and papers for solving nonlinear state vector estimation problems, including [3] and [6], which is commonly referred to as the propagation of the covariance matrix. As discussed later in this paper, and proved in the Appendix, the EKF covariance matrix propagation equation for propagating the covariance matrix is the only propagation method that, according to this paper, can be considered mathematically legitimate and not ad-hoc since it is mathematically equivalent to the Nonlinear Transition Matrix method featured in this paper. Thus, should the Unscented Kalman Filter [6], or any other single cycle recursive estimator outperform the EKF, a possible explanation is, for that particular problem, due to the lack of sufficient apriori knowledge about the state vector early in the scenario the state vector’s PDF could not be accurately approximated by a Gaussian.

When employing the full state formulation to the strapdown navigation problem, the EKF covariance matrix propagation method for transforming the covariance matrix from the previous cycle to the current cycle is numerically inefficient since it requires two matrix multiplications for each navigation cycle. This often translates into hundreds or even thousands of matrix multiplications per state vector update-cycle. According to a simulation study, due to numerical round-off errors resulting from all these required multiplications (even when using double precision arithmetic), the EKF would often become unstable, i.e. the estimation errors would eventually start to diverge. However, by replacing the EKF propagation method with the Nonlinear Transition Matrix Method, which requires only two (not hundreds of) matrix multiplications per state vector update-cycle, the implemented EKF was very stable. It is therefore suggested that the reason the navigation estimation pioneers adopted the low fidelity linear error-model formulation, and not the high fidelity nonlinear full state formulation, can be traced to the unnecessary use of the EKF propagation method for transforming the covariance matrix from the previous cycle to the current cycle. In other words, although the strapdown navigation estimation pioneers were able to construct the transition matrix for their adopted linear error models, they either not know how, or did not have the computer power, to construct the Nonlinear Transition Matrix for high fidelity nonlinear full state models.

In addition to numerical reasons, there is a significant theoretical reason for evaluating the Nonlinear Transition Matrix. As shown in the derivation presented in Section 4, transforming the covariance matrix has no theoretical significance when attempting to solve the more general (uncertainties in the GRV parameters of the differential and senor measurement equations are correlated in time) single cycle recursive estimation problem, but the solution still depends the evaluation of the Nonlinear Transition Matrix or its inverse. However, it appears that the pioneers of real-time estimation chose to formulate their problems so that these uncertainties were not time correlated. It is suggested that they may have done this so that the Kalman filter, which they were obviously trying to promote, can be given the status of being an optimal solution to the linear estimation problem. A derivation of the EKF equations, that starts with the definition of the single cycle recursive estimation problem and it standard limitations, is presented in Section 1. Referring to the first quote in the first paragraph of this section, it will be left to the reader to decide whether or not this derivation can be considered a “theoretically elegant argument” for implementing the EKF in the manner suggested. The derivation is based upon a new single cycle estimation methodology, referred to as the Sequential Maximum Likelihood Estimator (S-MLE). Unlike the EKF which is an extension of the Kalman Filter, the solution to the special linear estimation problem, the S-MLE is a sequential data processing modification to the MLE. However, in addition to employing the basic processing concepts of the MLE, the S-MLE includes a technique that makes it possible to handle estimation problems that contain nondeterministic dynamics. In the S-MLE formulation of the navigation estimation problem, nondeterministic dynamics implies the use of inertial acceleration and angular-velocity models that contain known parameters (i.e. IMU inputs to the problem) that are treated as Gaussian Random Variables (GRVs). Section 1 contains the derivation of the EKF equations. The evaluation of the Nonlinear Transition Matrix for the 33 state vector strapdown navigation estimation problem and the derivation of the expression for the Q-matrix for the general strapdown navigation estimation problem are described in Section 2. Section 2 also describes the real-time S-MLE implementation of the EKF equations to the strapdown navigation estimation. Section 3 contains the comparison results between simulated Monte Carlo tabulated RMS estimation errors and the corresponding sigma values of the Cramer-Rao Lower Bound matrix. These comparison plots provide the most direct possible evidence that the S-MLE is an optimal strapdown navigation state vector estimation technique. If the reader is interested in verifying the optimality of S-MLE for different scenarios, the 33 state vector strapdown navigation FORTRAN Monte Carlo simulation program can be e-mailed by contacting Shapiro at RECINCNET@AOL.COM. Section 4 contains a derivation of the S-MLE equations, a set of equations that can be used to solve the more general ( single cycle recursive estimation problem. Section 1 Derivation of the EKF Equations

The implementation and derivation of the S-MLE equations for the navigation state vector estimation problem is described in an inertial coordinate system. The processing that describes the transformation of the inputs and outputs of the problem between the user’s Earth-fixed rotating coordinate system and the estimator’s inertial coordinate system is not discussed. The goal of the problem, as presented in this formulation, is to determine the covariance and mean value of the Gaussian approximation to the actual PDF that results from the linearization of the dynamic and sensor measurement models. It is assumed that the reader is familiar with general form of the Gaussian PDF, which for the GRV r is given by

			(1.1)

. It is suggested the reader verify that Eq. (1.1) represents a mathematically consistent expression by proving the bottom two integral identities given the Gaussian expression for the PDF. The corresponding LLF for r is the scalar quadratic expression

Because this is a new formulation, understanding its mathematical constructs is essential. Section 1.1 contains four tables that describe these constructs. The starting point for the derivation of the Kalman filter equations are two vector equations that relate the unknown state vector at cycle k to the two known GRV vector input values to the problem. These two input GRV vectors are the estimated value for the state vector at cycle k-1 and the sensor data vector for cycle k. Section 1.2 describes the linearization approximations made to the Backward Trajectory Equation, an equation that represents the relationship between the unknown state-vector at cycle k and the known input GRV state vector at cycle k-1. Section 1.3 describes the linearization approximation made to the Sensor Measurement Equation, an equation that represents the relationship between the unknown state vector at cycle k and the known GRV sensor data vector at cycle k. Using these two linearized equations, Section 1.4 contains a derivation of the log-likelihood function (LLF) that defines the Gaussian PDF for the unknown incremental update state vector. Section 1.5 uses this LLF expression to derive the EKF equations.

Section 1.1 Tables of Mathematical Constructs

The mathematical constructs used in Section 1 are grouped by the following 4 tables: (1) known inputs to the problem (2) initially unknown desired outputs of the problem (3) state-vector derivative functions (4) state-vector trajectory or integral functions. Since the S-MLE’s treatment of sensor measurement function is the same as the standard treatment, the mathematical constructs belonging to this function are not mentioned in the tables. TABLE 1 Generic and Navigational inputs to the problem which begins at cycle k-1 and ends at cycle k Row Symbol Name & Description 1 Previous estimation cycle time. 2 , , Dynamic navigational inputs. Estimated values for the inertial position and velocity vectors and the body direction cosine matrix for time .

3 Static navigational inputs. Estimated values for the parameters that define the sensed-to body Inertial Function. In practice they represent errors in the pre calibrated or apriori values. In the simulation it was a 24 dimensional vector corresponding to IMU biases, scale-factors and body misalignment-angles. 4 The generic input estimated state vector. Represents the mean value for GRV state vector at time . The catenation of dynamic and static estimation parameters.

5 Input covariance matrix for .

6 Control or IMU readings – In navigation it represents mean value of IMU sensed inertial acceleration and angular velocity GRV vectors at time t. 7 The control vector covariance matrix or IMU covariance matrix at time t. Represents the covariance matrix for the IMU sensed inertial acceleration and angular velocity vectors. 8 Current estimation cycle time 9 Generic input sensor data vector. Mean value for the input sensor data vector GRV, .

10 Measurement Covariance Matrix. The covariance matrix for .

TABLE 2 Desired outputs of the problem which begins at cycle k-1 and ends at cycle k Symbol Name & Description

Current estimated state vector-- Catenation of -- represents the estimated mean value of the Gaussian approximation to unknown state vector, , at time

Current covariance matrix – represents the covariance matrix for the Gaussian approximation to the theoretical PDF( ).

TABLE 3 Mathematical Constructs Related to the State Vector Derivative Function Row Symbol Name & Description 1 Control uncertainty vector. In navigation it represents the uncertainty in the IMU sensed inertial acceleration /angular-velocity 2 Control random variable vector. For the navigation problem it represents the GRV values for the IMU sensed inertial acceleration and angular-velocity vectors at time t. 3 Control GRV covariance matrix for two arbitrary times. 4 State-Vector Derivative Function, . Defines the derivative of the state vector. A piecewise continuous integrable function. 5 Deterministic State-Vector Derivative Function. Represents the mean value for the State-Vector Derivative Function . 6 Uncertainty Derivative Jacobian. The Jacobian of the State-Vector Derivative Function wrt evaluated at and

7 Non-deterministic derivative function. A linear approximation to the statistical uncertainty in the derivative function.

TABLE 4 Mathematical Constructs Related to State Vector Trajectory Function Row Symbol Name & Description 1 Trajectory function. Non-deterministic mapping of the state vector at any arbitrary time to any other arbitrary time .

2

Deterministic Trajectory Function. Unique mapping of the state vector at any arbitrary time to the state vector at any other arbitrary time .

3 Forward Trajectory Function. Unique mapping of the state vector at time to the state vector at time (only trajectory function implemented when implementing the RKF equaitons) 4 Backward Trajectory Function. Unique mapping of the state vector at time to the state vector at time . Used in the derivation of the EKF equations. 5 Nonlinear Transition Matrix. The Jacobian of the estimated Forward Trajectory Function. 6 Inverse-Nonlinear Transition Matrix. The Jacobian of the estimated Backward Trajectory Function.

7 Uncertain Backward Trajectory Vector. A zero-mean GRV. In the strapdown navigation it represents a linear approximation to the uncertainty in the Backward Trajectory Function due to IMU white noise errors. 8 The Backward Trajectory Q-Matrix.

It is noted that for an estimation problem that has linear dynamics the Forward Trajectory Function (Table4-Row3) is given by . Thus, the Jacobian that defines the Nonlinear Transition Matrix (Table4 Row5) also defines the standard linear transition matrix. It is also noted that the Forward and Backward Trajectory Functions (Table4-Row4) are inverse functions of each other and therefore the Backward Transition Matrix and the Nonlinear Transition Matrix are inverse the matrices of each other. When implementing the EKF equations only the Forward Trajectory Function needs to be evaluated. However, in the derivation of the EKF equations, the Backward Trajectory Function is employed since it maps the unknown state-vector at cycle k to the known state-vector at cycle k-1.

Section 1.2 Linearized Backward Trajectory Equation

The Backward Trajectory Equation is obtained by setting the Backward Trajectory Function (Table4-Row4) to the GRV input value for state vector at cycle k-1

 			(1.2)

Linearizing the left hand side of the above about the mean value for u(t) (Table3-Row2) and then replacing with in the Jacobian term results in

,  ( Table3-Row5)	(1.3)

Linearizing the left hand side of the above about the propagated input state vector (Obtained by using the Forward Trajectory Function- Table4-Row3) produces

  (1.4)

Since equals the input state-vector, , the above equation simplifies to the Linearized Trajectory Equation which has the unknown incremental update vector, , on the left hand side

   					(1.5)

Equation (1.5) defines the uncertainty in the incremental upstate vector before the processing of the sensor data. It is one of only two equations (see Eq. (1.7) below) that defines the PDF for . It therefore makes sense that, in order to achieve the optimal performance in the implementation of the EKF equations, the evaluation of the Nonlinear Transition Matrix and the Q-matrix should be preformed accurately. The only possible inaccuracy of this equation would be due to the linearization approximation made in going from Eq. (1.3) to Eq. (1.4). Since it is assumed that the state vector’s PDF can be accurately approximated by a Gaussian implies that the backward trajectory function can be accurately approximated by Eq. (1.4), its first order Taylor Series expansion. If, in reality, this approximation is not valid then the best way to solve the problem would be to use a batch processing technique.

Section 1.3 Linearized Sensor Measurement Equation The sensor-measurement-equation is obtained by setting the EKF sensor-measurement-function equal to the input sensor vector

						(1.6)

Linearizing about results in desired linear GRV equation with the unknown vector on the left hand side

				(1.7)

Section 1.4 Log-Likelihood Expression for the Incremental Update Vector

This section assumes the reader is familiar with the form of the probability density function for GRVs (see Eq. (1.1)). Combining Eq. (1.5) and Eq. (1.7)  produces an over determined set of linear GRV equations (an equation that is linear in the unknown vector and contains known constants on the right hand side of the equation represented by GRVs) which can be expressed in the following matrix-vector equation form
				(1.8)

The limited single cycle recursive estimation problem makes the assumption that the zero mean GRVs vectors are uncorrelated (the same assumption made by the EKF) which implies that the vector is a GRV with a mean value and covariance matrix given by

		(1.9)

Thus the Log-likelihood function for b, in terms of its mean value and covariance matrix, is given by

		(1.10)

Using Eq. (1.8) and replacing with in Eq. (1.10), produces the following quadratic expression for the LLF of the incremental state vector

	(1.11)

Collection terms in powers of , the above can be expressed as

	(1.12)

Section 1.5 EKF Equations The linearization of the two basic equations (1.2) and(1.6), and then performing the necessary algebra, produced the expression for covariance matrix of the Gaussian approximation to the PDF as shown in Eq. (1.12). Since the mean value for the Gaussian approximation is the same value that minimizes the LLF, the mean value is obtained by taking the gradient of (1.12) wrt , setting the result to zero, and then performing the necessary algebra. This results in the following matrix-vector equation that is mathematically equivalent to the EKF equations

				(1.13)

Section 2 contains a derivation for the different elements of the nonlinear transition matrix, , and the Forward Trajectory Q-matrix, , for the 33 state vector strapdown navigation estimation problem.

It is pointed that the only reason the term (the mathematical equivalence to the propagation of the covariance matrix) is present in Eq. (1.13) is because the off-diagonal sub matrices of the covariance matrix in (1.9) are zero. They are zero do to same limiting assumption that makes the Kalman filter an optimal solution to the linear estimation problem, the uncertainty is the input sensor data vector and derivative functions are not time correlated. Section 4 contains a derivation of a set of equations that can be used to optimally solve a more general single cycle estimation problem, i.e. a solution that should provide Monte Carlo RMS estimation errors that are statistically consistent with the calculated CRLB, even after removing the time uncorrelated assumptions. Section 2 Derivation of the Nonlinear Transition Matrix and Q-Matrix

The IMU readings represent an imprecise measurement of the body-inertials (short for inertial acceleration and inertial angular-velocity). Part of this imprecision is due to white noise measurement errors and part is due to the apriori and slowly time varying values in the pre lab-calibrated IMU parameter values, e.g. biases, scale-factors and misalignment angles, which are used in the lab generated pre-calibrated mapping of the IMU sensed-inertials to the idealized body-inertials . In the S-MLE full state formulation a derived expression for the Q-matrix is used to account for the white noise measurements errors and the real-time estimated values for the IMU parameters are used to account for the error in the sensed-to-body inertial function due to inaccurate apriori values of the IMU parameters. Expressed in terms of the 24 static components of the state vector, this 6-dimensional functional mapping is given by (2.1) Sensed-to-Body Inertial Function

The following is a block diagram of the implemented deterministic dynamic model

SIMULATED FULL STATE DYNAMIC MODEL

The Nonlinear Transition Matrix and the Q-matrix are two fundamental EKF matrices that traditionally are either not properly calculated and not properly utilized. In fact, the common practice is to design the Q-matrix in order to compensate for a suboptimal implementation of the deterministic transformation of the input covariance matrix from the previous cycle to the current cycle, represented in this paper by . For the 33 state vector strapdown navigation estimation problem, the derived expression for the Nonlinear Transition Matrix is presented in section 2.1 and the derived expression for the Q-matrix is presented in section 2.2. When used in an estimation problem that contains nonlinear dynamics the term is referred to as the S-MLE Nonlinear Transformation Expression. Section 2.1 The Derivation of the Full State Nonlinear Transition Matrix. The mathematical constructs that are used in this section are contained in Table 5 on the next page. Utilizing the following mathematical identities for the double integral and the derivative of a direction cosine matrix, , the deterministic or mean derivative of the 15 component dynamic vector (consisting of only 9 independent components) is given by

	 (2.2)

TABLE 5 Row Symbol Name & Description

1 IMU readings. They represent the mean value for the sensed inertial acceleration, , and angular-velocity , , vectors at time t.

2 IMU calibration parameter vector. The catenation of the accelerometer ( ) and gyro ( ) biases, scale factors and misalignment angles.

3 Body direction cosine matrix. The columns represent the body axes expressed in the inertial coordinate system. Used in transforming a vector expressed in the body axes to the same vector expressed in the inertial axes.

4 Rotation operator “ ”. Rotates vector “ ”about the rotation vector“ ”, with “ ” expressed in radians. Its implementation is not discussed in this paper. The vector cross product is denoted by “ ”

5 Reference direction cosine matrix (i.e. the identity matrix). Represents vehicle’s direction cosine matrix when perfectly aligned with the inertial axes.

6

= 

Body rotation vector “ ”. The three dimensional rotation vector which, when applied to the inertial bases vectors, results in the body’s direction cosine matrix at time t. 7 The inverse rotation operator “ ”. Determines the rotation vector of the rotation operation that, when applied to the previously known direction cosine matrix , results in the previously known direction cosine matrix

8 Direction-Cosine Dynamic State-Vector. A 15-dimensional vector given by the catenation of the inertial position, inertial velocity and the three body axes all expressed in an inertial Cartesian coordinate system.

9 Rotation Dynamic State-Vector. A nine dimensional vector given by the catenation of the inertial position, inertial velocity and the body rotation vector all expressed in an inertial Cartesian coordinate system. 10 Deterministic Direction-cosine Derivative Function. Defines the derivative of the Direction-cosine dynamic state vector. 11 Deterministic Rotation Derivative Function. Defines the derivative of the deterministic Rotation Dynamic State-Vector.

12 Sensed-to-Body Inertial Function. The mapping of the sensed-inertials to the body-inertias(2.1).

13 Deterministic Inertial function. The mapping of the mean sensed-inertials to the mean inertial-inertials expressed in terms of 27 components of the k-1 state vector 14 Acceleration due to gravity.

15 Estimated Forward Trajectory Function. The mapping of the estimated state vector at cycle k-1 to the pre-estimated state vector at cycle k.

16 The ith component of the jth incrementally changed state vector ( ). Except for the jth component, it is equal to the input estimated state vector for cycle k-1. 17 jth incrementally Forward Trajectory Function. Represents the dynamic model mapping of ( ) to the state vector at cycle k

The implementation of the estimated forward trajectory function {Table4-Row3} is performed using the 15 component direction-cosine dynamic state vector, which is expressed by

  (2.3)

In terms of the mathematical construct defined in Table 5 (rows 15-17), the i-j element of the S-MLE transition matrix is calculated according to

		(2.4)

The simulated above 33x33 S-MLE transition matrix can be expressed in terms of four sub Jacobeans.

		(2.5)

Using to represent the inertial bases vectors, it can be shown that the upper left Jacobian is given by

	(2.6)

The last 24 columns of the first 6 rows of the transition matrix are implemented according to

The columns 10:21 of rows 7,8,9 are zero since the direction cosine matrix does not depend upon the accelerometer parameters. The columns 22:33 of rows 7,8,9 are implemented using the following integrals along with the inverse rotation operator “ ” (Table5- Row7)

	(2.7)

Section 2.2 Derivation of the Q-matrix The mathematical constructs used in the derivation of the S-MLE Q-matrix are presented in Table 6 on the next page. According to (2.1) , the sensed -to-body inertial function can be expressed as

(2.8)

TABLE 6 Mathematical constructs used in the derivation of the strapdown navigation Q-matrix Row Symbol Name & Description 1 Navigation time interval 2 Uncertain Sensed Inertial Vector. Zero-mean GRV representing uncertainty in the sensed-inertials due to IMU white noise errors. 3 Uncertain Body Inertial Vector. Represents the uncertainty in the body-inertials due to IMU white noise errors..

4 Uncertain Inertial Vector. Represents the uncertainty in the inertial-inertial due to IMU white noise errors.

5 IMU sensed accelerometer/ angular-covariance matrix.

6 Nondeterministic Derivative Function.

7 Nondeterministic Backward Trajectory Function. Represents a linear approximation to the statistical uncertainty in the Backward Trajectory Function. A zero mean nine dimensional GRV

8 Backward Trajectory Covariance Matrix. A 9x9 matrix.

Taking the partial derivative of Eq. (2.8) wrt and , produces the following expression for the Uncertainty Body Inertial Vector and Uncertainty Inertial Vector {Table6-Row3&Row4}

		(2.9)

This results in the following expression for the Nondeterministic Derivative Function (Table6-Row6)

			(2.10)

Substituting Eq. (2.10) into the general expression for the 9x9 none zero elements of Backward Trajectory Covariance Matrix produces

	(2.11)

After performing the integration of t’, the Backward Trajectory Covariance Matrix (expressed in the terms of a 33x33 matrix) is given by.

	(2.12)

The minus sign occurs because the t’ integration over the delta function was in the negative direction. Referring back to Eq. (1.13), the Forward Trajectory Covariance Matrix is given by

					(2.13)

Referring back to Eq. (2.6), the 9x9 upper left Jacobian of the Nonlinear Transition Matrix is given by

 [1:9, 1:9] = 				(2.14)

Since is not a function of time it can be brought inside the integral and since is a 3x3 matrix it commutes with . Performing the multiplication

Thus the desired expression for the 33x33 Forward Trajectory Covariance Matrix or Q-matrix is

	(2.15)

The negative sign is not present because the integration limits were reversed in going from Eq. (2.12) to Eq. (2.15). Section 2.3 Realtime Practical Considerations Careful examination of Eq. (2.5), the evaluation of the Nonlinear Transition Matrix, and Eq. (2.15), the evaluation of the Forward Trajectory Covariance Matrix, requires the implementation of numerous numerical time integrals that start at the previous cycle and end at the current cycle. However, all this integrals, many of which depend upon the IMU readings, can be performed in parallel during the real-time processing of the IMU input data. Thus the running sum of these numerical integrals can be updated as soon as the IMU information for a given navigation time interval becomes available. This means that, if the time interval between state vector update cycles is one second, then the processing time that is available to perform these integrals is also one second. Thus, for those strapdown navigation systems that have reasonable processing throughput, the formation of the Nonlinear Transition Matrix and the Forward Trajectory Covariance Matrix should not be a real-time processing concern. Section 3 Evidence of the Optimality of the S-MLE Estimator This section contains 9 Excel embedded charts that compare the Monte Carlo tabulated RMS state vector estimation errors with the calculated theoretical Crammer-Rao Lower Bound (CRLB) sigma value for 101 estimation cycles. Thus the charts represent only the first 100 seconds of the scenario. The zero cycle represent the errors in the apriori estimate. The Monte Carlo tabulated average value is presented to demonstrate that the estimator was a biased estimator, explaining why it was possible for the RMS estimation errors to be smaller than the CRLB limit. In the table on the next page, which is used to help explain the comparison charts, the symbol represents the estimated value and the symbol represents the simulated true value. The inverse rotation function (Table5-Row7), applied to the estimated and true direction cosine matrices was used in the evaluation of the RMS estimation attitude errors. The CRLB matrix used in this study was the S-MLE output covariance matrix when all simulated zero mean Gaussian Random Variables were set to zero (i.e. the scenario when there were no simulated errors in the GPS position and velocity sensor data, the magnetometer sensor data, the IMU sensor data and apriori state vector information). Anyone truly interested in verifying the optimality of S-MLE estimator should contact Shapiro.

EXPALANTION OF COMPARISON CHARTS Name Root Mean Squared estimation error value for each cycle CRLB sigma values Position

Velocity

Attitude

Accelerometer Bias

Accelerometer Scale-Factor

Accelerometer Misalignment Angles

Gyro Bias

Gyro Scale-Factor

Gyro Misalignment Angles

Section 4 General Solution to the Sequential Data Processing Estimation Problem The EKF equations were derived under the assumptions that the uncertainty in the sensor measurement data and the uncertainty in the in the derivative function were not correlated in time. The EKF equations were also derived under the limitation that, unlike the derivative function, the sensor measurement function cannot contain known parameters that can be treated as GRVs. This section contains a derivation of a set of equations, referred to as the S-MLE equations, that provide the mean value and covariance matrix for the Gaussian approximation to the PDF for the state vector for a the general single cycle recursive estimation problem. Table 7 contains the new mathematical constructs that are used in this derivation.

Row Symbol Defintion 1 n & m Number of components of the state vector & sensor data vector, respectively. 2 Sensor Uncertainty Parameter Vector. Zero mean GRV represents the uncertainty in the sensor measurement parameters that are treated as GRVs. 3 Sensor Measurement Function, expressed in terms of .

4 Sensor Parameter Covariance matrix. 5 Sensor Parameter Jacobian Matrix. 6 Sensor Argument Jacobian Matrix. 7 Total Jacobian Matrix. An (n+m)xn matrix representing the catenation of the Forward Trajectory Jacobian and the Sensor Argument Jacobian matrices. 8 State-Vector-Backward-Trajectory covariance matrix (nxn dimensional) 9 State-Vector-Sensor-Data covariance matrix (nxm dimensional). 10 State Vector-Sensor Function Covariance Matrix (nxm dimensional). 11 Mean Vector. Represents the mean value for the known (n+m) dimensional vector linearized vector.

12 Uncertainty Vector. Zero mean GRV (n+m) dimensional vector. Represents the uncertainty in the linearized known vector. 13 Total Uncertainty Covariance Matrix 14 Total Vector Space dot product operator. Describes how to form the dot product of two vectors (the mapping of two (n+m) dimensional vectors to a scalar) in the (n+m) dimensional Total Vector Space. Also used in matrix multiplication.

Since the input state vector GRV is a function of the statistics associated with senor data uncertainty vector, vk, and the uncertainty vector, w(t),it follows that if ,vk, and w(t) are time correlated, i.e. and/or , then the uncertainty in the input state vector, , would be correlated with vk and/or w(t). However, the EKF equations were derived under the assumption that was uncorrelated with vk and w(t) . The effect of these correlation are contained in the State-Vector-Backward-Trajectory covariance matrix (Table7- Row8) and the State-Vector-Sensor-Data covariance matrix (Table7-Row9). How these two matrices are to be evaluated is an area of future research, along with the evaluation of the State-Vector-Sensor-Function covariance matrix (Table7- Row10).

Expressing the dependence of the sensor measurement function on the state vector and the Sensor Uncertainty Parameter Vector (Table7 Row2) the sensor measurement Eq. (1.6) becomes

						(4.1)

Linearizing about equal zero and about equal results in the following linearized Sensor Measurement Equation for the incremental update vector (Table7 Row5 & Row6)

					(4.2)

Combining (1.5) with (4.2) results in the following set of equations

			(4.3)

The vector is a (n+m) dimensional zero mean GRV (Table7 Row11) and represents the uncertainty in the known GRV vector on the right hand side of the linearized equations described by Eq. (4.3). The (n+m)x(n+m) covariance matrix for this Total Uncertainty Vector is given by

		(4.4)

Following the same logic that led to the expression for LLF of the incremental update state vector given by Eq. (1.11), the LLF for the more general problem becomes

		(4.5)

Using the notation introduced in Table7 Rows 7, 11 &14, the LLF can be conveniently written as

  		(4.6)

Taking the gradient and setting to zero and solving for results in the following S-MLE equation for the solution to the estimated incremental update state vector

				(4.7)

Using Eq. (4.7) requires the evaluation of the inverse of the S-MLE Nonlinear Transformation Matrix, . An equivalent solution that does not require the evaluation is obtained by multiply both sides of (4.7) by and multiplying by (the identity matrix). This results in an expression for the incremental update vector for the state vector at cycle k-1

		(4.8)

After solving for , the update state vector for cycle k can be obtained from

						(4.9)

Conclusion

To use an analogy, S-MLE is to the EKF what Newtonian Physics is to Kepler’s Laws of planetary motion.  Newtonian physics explains Kepler’s laws through the law of gravity and the second law of motion.  The S-MLE explains the EKF through the definition of the problem and the basic concepts of the Maximum Likelihood Estimator. 

There is subjective evidence that, like Kepler who arrived at his laws through astronomical observations and not theoretical considerations, the EKF equations were arrived at through simulation studies and trial-and-error. Evidence for this can be seen in the derivation of the EKF covariance matrix propagation equation as presented in reference [3] Chapter 6. Somehow this book’s derivation, after making numerous assumptions and simplifications, arrives at the mathematically correct result as demonstrated by the S-MLE derivation of the EKF equations and the proof presented in Appendix A. The book goes on to say, in relationship to the EKF covariance matrix propagation equation, “Higher-order, more exact approximation to the optimal nonlinear filter can be achieved using more terms in the Taylor Series Expansion for the nonlinearities, and by deriving recursive relationships for higher moments of x.” However according to the S-MLE derivation the standard propagation equations are theoretically correct when the PDF can be approximated by a Gaussian. This is a clear indication that Gelb, and those that have chosen to implement the Unscented Kalman filter [7], and others who have tried to modify the EKF covariance matrix propagation equation and update equation in their attempt to solve nonlinear state vector estimation problems, have been unaware that the best way to solve some nonlinear problems is to apply, early in the estimation scenario, batch processing MLE techniques to the derived expression for the state vector’s PDF. If one accepts the EKF limiting assumptions, there is no theoretical justification to tamper with its equations although there can be, as suggested by this paper, implementations reasons for doing so.

If one compares the utilitarian value of Kepler’s laws of planetary motion with Newtonian physics and then applies this comparison to the EKF and S-MLE, the possible utilitarian value of the concepts presented in this paper could be enormous. Using the S-MLE to optimally solve the strapdown navigation estimation problem is just one indication of this.

Appendix A. Mathematical Equivalence of the EKF Covariance Matrix Propagation Equation and the S-MLE Covariance Matrix Transformation Expression.

The following shows that the time derivative S-MLE Covariance Matrix Transformation Expression results in the EKF covariance matrix propagation equation. This implies that S-MLE Covariance Matrix Transformation Expression represents a closed form expression for the integral of the covariance matrix propagation equation. This derivation does not discuss the Q matrix contribution to the EKF covariance matrix propagation equation which is non relevant to this discussion.

In the S-MLE covariance matrix transformation expression for symbol for tk is replaced by the symbol t, representing some time in the interval between tk-1 and tk. Making this change results in the following expression for the S-MLE covariance matrix transformation expression

 			A.1

Replacing by results in

Since can be expressed in terms of ,the above equation can be expressed as

 A.2

Substituting A.1 into A.2 results in

			 A.3

The value for is given by

It therefore follows that

 			A.4

A substituting A.4 into A.3 and performing the necessary algebra results in

 	A.5

Taking the limit as goes to zero, produces the desired expression for determinist part of the EKF covariance matrix propagation equation

References [1] http://www.quantlet.com/mdstat/scripts/mva/htmlbook/mvahtmlnode46.html [2] http://en.wikipedia.org/wiki/Maximum_likelihood_estimator [3] Gelb, A, "Applied Optimal Estimation," 1974 [4] http://homepages.inf.ed.ac.uk/rbf/CVonline/LOCAL_COPIES/WELCH/kalman.2.html

(see footnote)

[5] A New Formulation and Solution to the Navigation State Vector Estimation Problem, ION Proceedings NTM 2004 [6] Simon Julier, Jeffrey Uhlmann and Hugh Durrant-Whyte. New Approach for Filtering nonlinear Systems. In Proceedings of the American Control Conference, Vol. 3, pages 1628-1632, 1995.