20180003657.pdf

NASA STI Program. . . in Profile

Since its founding, NASA has been dedicated to the advancement of aeronautics and space science. The NASA scientific and technical information (STI) program plays a key part in helping NASA maintain this important role.

The NASA STI Program operates under the auspices of the Agency Chief Information O�cer. It collects, organizes, provides for archiving, and disseminates NASA’s STI.

The NASA STI Program provides access to the NASA Aeronautics and Space Database and its public interface, the NASA Technical Report Server, thus providing one of the largest collection of aeronautical and space science STI in the world. Results are published in both non-NASA channels and by NASA in the NASA STI Report Series, which includes the following report types:

• TECHNICAL PUBLICATION. Reports of completed research or a major significant phase of research that present the results of NASA programs and include extensive data or theoretical analysis. Includes compilations of significant scientific and technical data and information deemed to be of continuing reference value. NASA counterpart of peer-reviewed formal professional papers, but having less stringent limitations on manuscript length and extent of graphic presentations.

• TECHNICAL MEMORANDUM.

Scientific and technical findings that are preliminary or of specialized interest, e.g., quick release reports, working papers, and bibliographies that contain minimal annotation. Does not contain extensive analysis.

• CONTRACTOR REPORT. Scientific and technical findings by NASA-sponsored contractors and grantees.

• CONFERENCE PUBLICATION.

Collected papers from scientific and technical conferences, symposia, seminars, or other meetings sponsored or co-sponsored by NASA.

• SPECIAL PUBLICATION. Scientific, technical, or historical information from NASA programs, projects, and missions, often concerned with subjects having substantial public interest.

• TECHNICAL TRANSLATION. English-language translations of foreign scientific and technical material pertinent to NASA’s mission.

Specialized services also include organizing and publishing research results, distributing specialized research announcements and feeds, providing information desk and personal search support, and enabling data exchange services.

For more information about the NASA STI Program, see the following:

• Access the NASA STI program home page at http://www.sti.nasa.gov

• E-mail your question to help@sti.nasa.gov

• Phone the NASA STI Information Desk at 757-864-9658

• Write to:

NASA STI Information Desk Mail Stop 148 NASA Langley Research Center Hampton, VA 23681-2199

The use of trademarks or names of manufacturers in this report is for accurate reporting and does not constitute an o�cial endorsement, either expressed or implied, of such products or manufacturers by the National Aeronautics and Space Administration.

Available from:

NASA STI Program / Mail Stop 148 NASA Langley Research Center

Hampton, VA 23681-2199 Fax: 757-864-6500

NASA Engineering and Safety Center

Navigation Filter Best Practices

First Edition

J. Russell Carpenter and Christopher N. D’Souza, Editors with contributions from J. Russell Carpenter, Christopher N. D’Souza, F. Landis Markley, Renato Zanetti

April, 2018

This material is declared a work of the US Government and is not subject to copyright protection in the United States, but may be subject to US Government copyright in other countries.

If there be two subsequent events, the probability of the second b/N and the probability of both together P/N , and it being first discovered that the second event has also happened, from hence I guess that the first event has also happened, the probability I am right is P/b.

Thomas Bayes, c. 1760

The explicit calculation of the optimal estimate as a function of the observed variables is, in general, impossible.

Rudolph Kalman, 1960.

The use of Kalman Filtering techniques in the on-board navigation systems for the Apollo Command Module and the Apollo Lunar Excursion Module was an important factor in the overwhelming success of the Lunar Landing Program.

Peter Kachmar, 2002.

Dedicated to the memory of Gene Muller, Emil Schiesser, and Bill Lear.

Contents

Foreword iii

Editor’s Preface v

Notational Conventions ix

Chapter 1. The Extended Kalman Filter 1

1.1. The Additive Extended Kalman Filter 1

1.2. The Multiplicative Extended Kalman Filter 7

Chapter 2. The Covariance Matrix 9

2.1. Metrics for Orbit State Covariances 9

2.2. Covariance Propagation 15

2.3. Covariance Measurement Update 25

2.4. Sigma-Point Methods 28

Chapter 3. Processing Measurements 29

3.1. Measurement Latency 29

3.2. Invariance to the Order of Measurement Processing 30

3.3. Processing Vector Measurements 31

3.4. Filter Cascades 32

3.5. Use of Data from Inertial Sensors 32

Chapter 4. Measurement Underweighting 33

4.1. Introduction 33

4.2. Nonlinear E↵ects and the Need for Underweighting 34

4.3. Underweighting Measurements 38

4.4. Pre-Flight Tuning Aids 39

Chapter 5. Bias Modeling 41

5.1. Zero-Input Bias State Models 41

5.2. Single-Input Bias State Models 43

5.3. Multi-Input Bias State Models 49

Chapter 6. State Representations 53

6.1. Selection of Solve-For State Variables for Estimation 53

6.2. Units and Precision 53

6.3. Coordinate and Time Systems 54

6.4. Orbit Parameterizations 54

6.5. Relative State Representations 55

6.6. Modeling Inertial Components 58

i ii CONTENTS

Chapter 7. Factorization Methods 63

7.1. Why Use the UDU Factorization? 63

7.2. Preliminaries 64

7.3. The Time Update of the Covariance 65

7.4. The Measurement Update 72

7.5. Consider Covariance and Its Implementation in the UDU Filter 74

7.6. Conclusions 76

Chapter 8. Attitude Estimation 77

8.1. Attitude Matrix Representation 77

8.2. Euler Axis/Angle Representation 79

8.3. Quaternion Representation 80

8.4. Rodrigues Parameter Representation 83

8.5. Modified Rodrigues Parameters 84

8.6. Rotation Vector Representation 85

8.7. Euler Angles 86

8.8. Additive EKF (AEKF) 87

8.9. Multiplicative EKF (MEKF) 88

Chapter 9. Usability Considerations 93

9.1. Editing 93

9.2. Reinitialization, Restarts, and Backup Ephemeris 93

9.3. Ground System Considerations 94

Chapter 10. Smoothing 95

Chapter 11. Advanced Estimation Algorithms 99 The Sigma-Point Estimator 99

Appendix A. Models and Realizations of Random Variables 105

Appendix B. The Mathematics Behind the UDU Factorization 107 B.1. The Partitioning into Two Subproblems 107 B.2. The Mathematics Behind the Second Subproblem 107 B.3. The Agee-Turner Rank-One Update 109 B.4. Decorrelating Measurements 112 B.5. The Carlson Rank-One Update 112

Appendix C. An Analysis of Dual Inertial-Absolute and Inertial-Relative Navigation Filters 117

C.1. Introduction 117 C.2. The Filter Dynamics 117 C.3. Incorporation of Measurements 120 C.4. Analysis of the Merits of the Inertial-Absolute and Inertial-Relative Filters 123 C.5. Conclusions 125

Bibliography 127

Foreword

It certainly should not come as a surprise to the reader that navigation systems are at the heart of almost all of NASA’s missions, either on our launch vehicles, on robotic science spacecraft, or on our crewed human exploration vehicles. Clearly navigation is absolutely fundamental to operating our space systems across the wide spectrum of mission regimes.

Safe and reliably performing navigation systems are essential elements needed for routine low Earth orbiting science missions, for rendezvous and proximity operation missions or precision formation flying missions (where relative navigation is a necessity), for navigation through the outer planets of the solar system, and for accomplishing pinpoint landing on planets/small bodies, and many more mission types.

I believe the reader will find that the navigation filter best practices the team has collected, documented, and shared in this first edition book will be of practical value in your work designing, developing, and operating modern navigation systems for NASA’s challenging future missions. I want to thank the entire team that has diligently worked to create this NASA Engineering and Safety Center (NESC) GN&C knowledge capture report. I especially want to acknowledge the dedication, care, and attention to detail as well as the energy that both Russell Carpenter and Chris D’Souza, the report editors, have invested in producing this significant product for the GN&C community of practice.

It was Russell and Chris who had the inspiration to create this report and they have done a masterful job in not only directly technically contributing to the report but also coordinating its overall development. It should be mentioned that some high-level limited work was previously performed under NESC sponsorship to capture the lessons learned over the course of the several decades NASA has been navigating space vehicles. This report however fills a unique gap by providing extensive technical details and, perhaps more importantly, providing the underlying rationale for each of the navigation filter best practices presented here. Capturing these rationales has proven to be a greatly needed but very challenging task. I congratulate the team for taking this challenge on.

The creation, and the wide dissemination of this report, is absolutely consistent with the NESC’s commitment to engineering excellence by capturing and passing along, to NASA’s next generation of engineers, the lessons learned emerging from the collective professional experiences of NASA’s navigation system subject matter experts. I believe this book will not only provide relevant tutorial-type guidance for early career GN&C engineers that have limited real-world on the job experience but it should also serve as a very useful memory aid for more experienced GN&C engineers, especially as a handy reference to employ for technical peer reviews of navigation systems under development.

As the NASA Technical Fellow for GN&C I urge the reader (especially the “naviga-tors” among you obviously) to invest the time to digest and consider how the best practices provided in this report should influence your own work developing navigation systems for the Agency’s future missions. The editors and I recognize this will be a living document iii iv FOREWORD and we sincerely welcome your feedback on this first edition of the report, especially your constructive recommendations on ways to improve and/or augment this set of best practices.

Cornelius J. Dennehy NASA Technical Fellow for GN&C

January 2018

Editor’s Preface

As the era of commercial spaceflight begins, NASA must ensure that lessons the US has learned over the first 50 years of the Space Age will continue to positively influence the continuing exploration and development of space. Of the many successful strands of this legacy, onboard navigation stands out as an early triumph of technology whose continuous development and improvement remains as important to future exploration and commercial development as it was in the era of Gemini and Apollo. The key that opened the door to practical and reliable onboard navigation was the discovery and development of the ex-tended Kalman filter (EKF) in the 1960s, a story that has been well-chronicled by Stanley Schmidt [65], and Kalman filtering has far outgrown NASA’s applications over the inter-vening decades. What are less well-documented are the accumulated art and lore, tips and tricks, and other institutional knowledge that NASA navigators have employed to design and operate EKFs in support of dozens of missions in the Gemini/Apollo era, well over one hundred Space Shuttle missions, and numerous robotic missions, without a failure ever at-tributed to an EKF. To document the best of these practices is the purpose that motivates the contributors of the present document.

Kernals of such best practices have appeared, scattered throughout the open technical literature, but such contributions are limited by organizational publication policies, and in some case by technology export considerations. Even within NASA, there has heretofore not been any attempt to codify this knowledge into a readily available design handbook that could continue to evolve along with the navigation community of practice. As a result, even with the Agency, it is possible for isolated practitioners “not to know any better:” to fail to appreciate the subtleties of successful and robust navigation filter design, and to lack an understanding of the motivations for, and the implied cost/benefit trade, of many of the tried and true approaches to filter design.

Some limited progress toward filling this void has been made at a summary level in reports and briefings prepared for the NASA Engineering and Safety Center (NESC) [13].

In particular, one of a series of recommendations in Reference 13 “...directed towards the development of future non-human rated [rendezvous] missions...” included as its fourteenth recommendation the admonishment to “[u]tilize best practices for rendezvous navigation filter design.” This recommendation listed eight such practices, as follows:

a. Maintain an accurate representation of the target-chaser rel-ative state estimation errors, including an accurate variance-covariance matrix. This allows the filter to compute an appropriate gain matrix. It also aids the filter in appropriately editing unsuitable measurements.

b. Provide a capability for measurement underweighting that adapts to the current uncertainty in the filters state estima-tion error, as required to be consistent with the suboptimality v vi PREFACE of the navigation filters measurement update. E↵ective means for accomplishing this have been found to include:

i. Modified second-order Gaussian state update method [30];

ii. Multiplicative adjustment of the mapping of the state error covari-ance matrix into the measurement subspace, which occurs within the computation of the residual covariance [78]; and

iii. Schmidt-Kalman state update [3] that utilizes the covariance ma-trix of “consider” parameters (i.e., states that the filter does not update, but for which it maintains a covariance).

Multiplicative adjustment of the measurement noise covariance matrix within the computation of the residual covariance (the “bump up R” method [3]) has been found to be less e↵ective, and is not recommended unless other methods are not feasible.

c. Estimate states that model biases in sensor measurements and account for unmodeled accelerations. Gauss-Markov models for these biases have been found to be more e↵ective than random con-stant or random walk models. Random constant models can become stale, and random walk models can overflow during long periods with-out measurement updates.

d. Provide commands that allow for selective processing of indi-vidual measurement types. If the filter utilizes an automated resid-ual edit process, then the recommended command capability should be able to override the residual edit test.

e. Maintain a backup ephemeris, unaltered by measurement up-dates since initialization, which can be used to restart the filter without uplink of a new state vector.

f. Provide a capability for reinitializing the covariance matrix without altering the current state estimate.

g. Ensure tuning parameters are uplinkable to the spacecraft, and capable of being introduced to the filter without loss of onboard navigation data.

h. Provide flexibility to take advantage of sensors and sensor suites full capability over all operating ranges.

A subsequent briefing given for an NESC webinar [7] listed these as well as the following “additional considerations:”

• State Representation

– Translational states

⇤ Dual inertial ⇤ Inertial/relative ⇤ Relative-only

– Attitude states, as required ⇤ 3-parameter vs. 4-parameter ⇤ Multiplicative vs. additive update

• Covariance Factorization (or not)

– U-D

– “Square Root” Methods

• Measurement correlation

PREFACE vii

• Non-simultaneous measurements

• Backward smoothing (for BETs)

• Error Budgets

• Sensitivity Analysis

• IMU/Accelerometer processing

• Observability

While these summary-level lists give the community a place to start, they are lacking in some respects. They lack su�cient rationale that would motivate a designer to adopt them.

Even if so motivated, a designer needs much more detailed information concerning how to implement the recommendations.

The present work is an attempt to address these shortcomings. Each contributor has selected one aspect of navigation filter design, or several closely related ones, as the basis of a chapter. Each chapter clearly identifies best practices, where a consensus of the com-munity of practice exists. While it is sometimes di�cult to cast aside one’s opinions and express such a consensus, each contributor has made a best e↵ort in this regard. Where a diversity of opinion exists, the chapter will summarize the arguments for and against each approach. Also, if promising new developments are currently afoot, the chapter will assess their prospects.

While the contributors strive for consistency of convention and notation, each has his own preferences, and readers may need to accommodate subtle di↵erences along these lines as they traverse the book. The first chapter, which summarizes the EKF, sets the stage, and should be briefly perused by even seasoned navigators in order to become familiar with the conventions adopted for this work. Subsequent chapters should stand on their own, and may be consulted in any order.

While this is a NASA document concerned with space navigation, it is likely that many of the principles would apply equally to the wider navigation community. That said, readers should keep in mind that hard-earned best practices of a particular discipline do not always carry over to others, even though they may be seemingly similar. To assume so is a classic example of the logical fallacy argumentum ad verecundiam, or the argument from [false] authority.

Finally, the contributors intend for this work to be a living document, which will continue to evolve with the state of the practice.

Notational Conventions

A A set a,↵, A Scalars q,M An array of scalars, e.g. column, row, matrix x A point in an abstract vector space r A physical vector, i.e. an arrow in 3-D space

F A coordinate frame

MT The transpose of the array M kxk The (2-)norm of the vector x y A random variable z A random vector p x

(x) The probability density function of the random variable x evaluated at the realization x

Pr(y < Y ) The probability that y < Y

E[z] The expectation of the random vector z exp(t) The exponential function of t et exp(t) written as Euler’s number raised to the t power dx Leibniz’ (total) di↵erential of x dy dx (First) (total) derivative of y with respect to x dny dxn

= dn dxn y nth (total) derivative of y with respect to x Fdn dtn ~r nth (total) derivative of ~r with respect to t in frame F @M

@x

(First) partial derivative of M with respect to x

@M

@x x o

(First) partial of M with respect to x, evaluated at x o ix

CHAPTER 1

The Extended Kalman Filter

Contributed by J. Russell Carpenter

As described in the preface, use of the Extended Kalman Filter (EKF) for navigation has a long history of flight-proven success. The EKF thus forms the foundational best practice advocated by this work, and it forms the basis for many of the best practices later chapters describe. The purpose of the present chapter is not to derive the EKF and its relations, but rather to present them in a basic form, as a jumping o↵ point for the rest of the material we shall present. As we shall show, while the EKF is a powerful and robust algorithm, it is based on a few ad hoc assumptions, which can lead to misuses and misunderstandings.

Many of the best practices we shall describe are tricks of the trade that address such issues.

1.1. The Additive Extended Kalman Filter

The additive EKF is distinguished from the multiplicative EKF (MEKF) by the form of its measurement update. The additive EKF is the usual and original form of the EKF, and when we refer to the EKF without a modifier, one may assume we mean the additive form.

1.1.1. The Dynamics Model Suppose we have a list of n real quantities that we need to know in order to perform navigation, and we have a di↵erential equation that tells us how these quantities evolve through time, such as

Ẋ(t) = f(X(t), t) (1.1) where X 2 Rn, which we call the state vector, contains the quantities of interest; we shall call the f(X, t) the dynamics function. If we knew these quantities perfectly at any time, (1.1) would allow us to know them at any other time. For a variety of reasons, this is not the case however; both the initial conditions and the dynamics function are corrupted by uncertainty.

Suppose instead that the quantities of interest are realizations of a random process, X(t), whose distribution at some initial time t o is known to us, and whose evolution (forward) in time follows the stochastic di↵erential equation given by dX(t) = f(X(t), t) +B(t) dw(t) (1.2) where the presence of the process noise dw(t) reflects uncertainty in the dynamics. To interpret (1.2), imagine dw(t) as the limit of a discrete sequence of random increments, as the time between increments goes to zero. The result will be a continuous but non-di↵erentiable process; hence the notation Ẋ(t) has ambiguous meaning. Henceforth, we shall define our notation such that when we write

Ẋ(t) = f(X(t), t) +B(t)w(t) (1.3)

2 1. THE EXTENDED KALMAN FILTER

what we mean is really (1.2).

Finally, suppose that the initial distribution of X(t) is Gaussian, with mean and covari-ance given by

E[X(t o

)] = X̄ o and E

(X(t o

)� X̄ o

)(X(t o

)� X̄ o

)T

= P o

(1.4) and suppose that infinitesimal increments of w(t) are Gaussian, with

E[w(t)] = 0 and E[w(t)wT(⌧)] = Q(t)�(t� ⌧) (1.5) where Q(t) is the power spectral density function of w(t), and �(t � ⌧) is the Dirac delta function. We shall also assume that

E w(t)(X(t o

)� X̄ o

)T

= 0, 8 t (1.6)

We shall take (1.3) – (1.6) to define the dynamics model for the additive EKF. Note that even though we have assumed X(t o

) and w(t) are Gaussian, we cannot assume that X(t) remains Gaussian for t > t o

, because f may be a nonlinear function.

1.1.2. The Measurement Model In an ideal world, we might have devices for mea-suring all of the state vector components directly; then state determination would be simply a matter of collecting enough such observations to reduce the state uncertainty to su�cient levels. Unfortunately, this is almost never the case. Instead, like Socrates’ prisoners, we can usually only perceive noisy projections of the state elements, at discrete times, t i

, in the form of measurements:

Y(t i

) = h(X(t i

), t i

) + v(t i

) (1.7) where h is a surjection from Rn to Rm. We shall assume that v(t i

), which we call the measurement noise, is a Gaussian sequence, with mean and covariance given by

E[v(t i

)] = 0 and E[v(t i

)v(t j

)T] = R(t i ij

(1.8) where � ij is the Kronecker delta function. We shall also assume that

E v(t i

)(X(t o

)� X̄ o

)T

= 0 and E[w(t)v(t i

)T] = 0, 8 i, t (1.9)

We shall take (1.7) – (1.9) to define the measurement model for the additive EKF. Note that we cannot assume that Y(t i

) is Gaussian, because h may be a nonlinear function. For compactness of notation, we shall often suppress the time argument and write (1.7) as

Y i

= h i

(X i

) + v i

(1.10)

Bayes’ Law, the Markov Property, and Observability Bayes’ Law tells us how to update the conditional probability density function (PDF) of X(t i

), given a realization Y (t i

) of the random process Y(t i p X i

|Y i

(X i

|Y i

) = p Y i

|X i

(Y i

|X i p X i

(X i p Y i

(Y i

(1.11)

If all the PDFs in (1.11) were known, it would be relatively simple to use (1.11) to estimate the state vector from a single measurement; our best estimate of the state would simply be the mean of p

X i

|Y i

(X i

|Y i

). But to apply (1.11) to the navigation problem, where we have a time sequence of measurements, Y i

= {Y i

,Y i�1, . . . ,Y1}, we need to consider how the state dynamics evolve.

Unlike (1.1), our dynamics model, given by (1.3), only runs forward in time. Hence, the state at any future time depends only on its history. Also, because the non-homogeneous inputs to (1.3) are uncorrelated by the Dirac function in (1.5), the value of the state at

1.1. THE ADDITIVE EXTENDED KALMAN FILTER 3

any particular time in the future depends only on its present value, and its accumulated di↵usion due to the process noise over the interval between now and the future time of interest. Random processes such as this are said to possess the Markov Property. Using this property, we can write (1.11) in terms of the measurement history as follows:

p X i

|Y i

(X i

|Y i

) = p Y i

|X i

(Y i

|X i p X i

|Y i�1

(X i

|Y i�1) p Y i

|Y i�1

(Y i

|Y i�1)

(1.12)

Even if we could compute all of the PDFs in (1.12), we are not guaranteed that the sequence of measurements provide su�cient information to reduce the initial uncertainty of all the modes of (1.1). If the system given by (1.1) and (1.10) is such that use of (1.12) results in uncertainty in all the modes going asymptotically to zero in finite time, from any initial condition, then we say the system is globally asymptotically observable. If at least all of the unstable modes are observable, then we say the system is detectable. Unfortunately, for nonlinear systems, there is no known way to compute global observability. At best, under certain restrictions on (1.1) and (1.10), we can in principal establish local observability, in the neighborhood of a particular initial condition. However, this is a laborious calculation, often numerically unstable to evaluate. Also, note that observability is a property of the structure of (1.1) and (1.10), and hence is dependent on how one chooses to represent the navigation problem. Hence, a system that is observable with one representation may be unobservable with a di↵erent representation.

Kalman’s original filter, which we now usually call the linear Kalman filter (LKF), is the result when the dynamics and measurement models are linear, Markov, Gaussian, and observable. An appreciation of the linear Kalman filter is essential to understanding the strengths and weaknesses of the EKF, although it is almost never the case that such assumptions are valid for real-world navigation problems.

1.1.3. The Linear Kalman Filter Suppose the dynamics and measurements are given by the following discrete-time linear models:

x i i,i�1xi�1 + �i u i

(1.13) y i

= H i x i

+ v i

(1.14) with E[x o

] = x̄ o and E[(x o

� x̄ o

)(x o

� x̄ o

)]T] = P o

(1.15)

E[u i

] = 0 and E i u i u

T j

�T i

= S i ij

(1.16) and the moments of v i as given by (1.8). This system will be globally observable if the observability Gramian is strictly positive definite, W k k

X i=1

�0 i,1H

T i

H i i,1 > 0 (1.17) i.e. it has full rank.

With such assumptions, Kalman showed [36] that Algorithm 1.1 provides an optimal

(both minimum variance and maximum liklihood) estimate of the moments of the PDFs appearing in (1.11). Note that in Algorithm 1.1, the covariance recursion given by (1.19) and (1.22) does not depend on the measurement history, and hence one may compute the gain sequence, K i

, o↵-line and store it as a time-indexed table or schedule, along with � i,i�1 and H

. Also note that because the system is globally observable, there is no chance that it

4 1. THE EXTENDED KALMAN FILTER

Algorithm 1.1 The Linear Kalman Filter x̂ i i,i�1x̂ i�1, x̂

0 = x̄ o

(1.18)

P� i i,i�1P i�1�

T i,i�1 + S i

, P+ 0 = P o

(1.19)

K i

= P� i

HT

i

H i

P� i

HT

i

+R i

��1 (1.20) x̂ i

= x̂ i

+K i y i

�H i x̂ i

(1.21)

P+ i

= P� i

�K i

H i

P� i

(1.22) will fail to converge from any initial condition, except perhaps due to build up of numerical truncation and/or roundo↵ error.

If we further suppose the dynamics and measurements are given by linear time-invariant (LTI) models, x i

= �x i�1 + �ui (1.23) y i

= Hx i

+ v i

(1.24) then we may test its global observability using a somewhat simpler calculation than (1.17), as follows:

W =

H H� H�2

H�n�1

> 0 (1.25)

If the system is detectable, then it turns out that the covariance recursion given by (1.19) and (1.22) reaches a steady-state, which we denote P1. The corresponding gain is K1 = P1HTR�1. There exist numerous software packages that will compute such quantities, e.g. the Matlab Control Systems Toolbox, which may unfortunately lead to their misuse in inappropriate contexts. Perhaps worse, experts from other domains, who are familiar with techniques such as pole placement for control of LTI systems, may recognize that the steady-state linear Kalman filter is “just a pole placement algorithm,” and may infer that the EKF is not much more than a clever pole placement algorithm as well. As we shall show below, this is far from being the case; the EKF operates directly on the nonlinear system of interest, for which such LTI concepts have dubious applicability.

1.1.4. The Linearized Kalman Filter An immediately apparent generalization of the linear Kalman Filter is to use it to solve for small corrections to a nonlinearly propagated reference trajectory. While such an approach may have certain applications over limited time horizons, and/or for ground-based applications where an operator can periodically intervene, experience with onboard navigation systems has shown that such corrections can fail to remain small enough to justify the required approximations.

1.1.5. The Extended Kalman Filter There are a number of ways to proceed from Algorithm 1.1 to “derive” the EKF, but all contain a variety of ad hoc assumptions that are not guaranteed to hold in all circumstances. Most weaknesses and criticisms of the EKF arise from such assumptions. Rather than reproduce one or more of such derivations, we will simply point out that if one replaces (1.18) with an integral of (1.1) over the time between measurements, and computes the coe�cient matrices appearing in (1.13) and (1.14)

1.1. THE ADDITIVE EXTENDED KALMAN FILTER 5

as Jacobians evaluated at the current solution of (1.1), then the result is Algorithm 1.2, which bears more than a passing resemblance to the Kalman filter.

Algorithm 1.2 A Naive Extension of the Kalman Filter

X̂ i

Z t i t i�1 f(X(⌧), ⌧) d⌧, X(t i�1) = X̂ i�1, X̂

0 = X̄ o

(1.26)

A(t) = @f

@X

X̂(t)

, H i

@h i

@X

X̂ i

(1.27)

�(t i

, t i�1) =

Z t i t i�1

A(⌧)�(t i

, ⌧) d⌧, �(t i�1, ti�1) = I (1.28)

S i

Z t i t i�1

Z t i t i�1

�(t i

, ⌧)B(⌧) E[w(⌧)wT(�)]BT(�)�T(t i

,�) d⌧ d� (1.29)

P� i

= �(t i

, t i�1)P i�1�

T(t i

, t i�1) + S i

, P+ 0 = P o

(1.30)

K i

= P� i

HT

i

H i

P� i

HT

i

+R i

��1 (1.31)

X̂ i

= X̂ i

+K i

Y i

� h i

(X̂� i

(1.32)

P+ i

= P� i

�K i

H i

P� i

(1.33)

Several observations are in order regarding Algorithm 1.2.

• One might infer from (1.26) that X̂ i

= E[X|Y i�1]. This would be a somewhat problematic inference however, since in general

Z t i t i�1 f(E[X(⌧), ⌧)] d⌧ 6= E

Z t i t i�1 f(X(⌧), ⌧) d⌧

(1.34)

This implies that an initially Gaussian distribution for the state cannot in general remain Gaussian. At best, all we can hope is that X̂� i

⇡ E[X|Y i�1].

• Let us define the estimation error as e(t) = X(t) � X̂(t). Then since X̂(t) 6= E[X(t)|Y i�1], P(t) 6= E[e(t)eT(t)|Y i�1] (1.35)

At best, all we can hope is that P(t) ⇡ E[e(t)eT(t)|Y i�1].

• Let us define the innovation as r i

= Y i

� h(X̂� i

). Then since h(X̂� i

) 6= E[Y], H i

P� i

HT

i

+R i

6= E[r i r

T i

] (1.36)

At best, all we can hope is that the above will hold approximately.

• Taken together, the approximations listed above imply that (1.32) and (1.33) can at best satisfy (1.12) only approximately, not only because the mean and covariance are approximations, but also because the PDFs fail to remain Gaussian, and hence fail to be characterized completely by only their first two moments.

• Even if all of the above are reasonable approximations, there is a problem with (1.33). The posterior covariance should be approximated by

P+ i

⇡ E e i

(e+ i

)T|Y i

(1.37)

6 1. THE EXTENDED KALMAN FILTER

Let us assume that

Y i

� h i

(X̂� i

) ⇡ H i e i

+ v i

(1.38)

Then (1.32) implies that e i

= e i

�K i

H i e i

�K i v i

(1.39) and by our prior assumption that E e i v

T i

= 0, P+ i

⇡ E e i

(e+ i

)T|Y i

(1.40)

= (I�K i

H i

)P� i

(I�K i

H i

)T +K i

R i

KT

i

(1.41)

Equation (1.41) is Joseph’s Formula, and it holds for any gain K i

. Only for the optimal gain and true covariance does (1.41) reduce to (1.33). Since (1.31) was computed with only an approximate covariance, and due to the various other approximations listed above as well, K i cannot be the optimal gain, so at best, (1.33) will only hold approximately. At worst, such approximations may lead to P+ i becoming non-positive definite, which is a significant issue. Because of its symmetric and additive form, (1.41) is much less likely (but not impossible!) to produce a non-positive definite P+ i

• By our assumption that E[w(t)wT(⌧)] = Q(t)�(t� ⌧), one of the integrals in (1.29) should be annihilated by the Dirac function, resulting in

S i

Z t i t i�1

�(t i

, ⌧)B(⌧)Q(⌧)BT(⌧)�T(t i

, ⌧) d⌧ (1.42)

In any case, unlike for a discrete time dynamics model, E

Z t i t i�1

Z t i t i�1

�(t i

, ⌧)B(⌧)w(⌧)wT(�)BT(�)�T(t i

,�) d⌧ d�

6= E

Z t i t i�1

�(t i

, ⌧)B(⌧)w(⌧) d⌧

Z t i t i�1 w

T(⌧)BT(⌧)�T(t i

, ⌧) d⌧

(1.43)

• In general, one would need to simultaneously integrate (1.26) and (1.28) due to their interdependence via the Jacobian A. If the time between measurements is small enough, then if one were to employ a suitable approximation for (1.28), perhaps as simple as

�(t i

, t i�1) ⇡ I+A(t i

) (t i

� t i�1) (1.44) then one may reasonably expect that a carefully chosen approximation would be no worse than the many other approximations inherent in the EKF. One may also consider the same or simpler approximations when considering approximations to (1.42).

• Because there no way to prove global observability for a nonlinear system, the EKF may fail to converge from some initial conditions, even if the system is locally observable in particular neighborhoods.

In light of the above observations, we conclude this section by presenting a slightly improved version of the EKF as Algorithm 1.3. In subsequent chapters, we shall describe additional improvements to the EKF.

1.2. THE MULTIPLICATIVE EXTENDED KALMAN FILTER 7

Algorithm 1.3 A Slightly Improved Extension of the Kalman Filter

X̂ i

Z t i t i�1 f(X(⌧), ⌧) d⌧, X(t i�1) = X̂ i�1, X̂

0 = X̄ o

(1.45)

A(t) = @f

@X

X̂(t)

, H i

@h i

@X

X̂ i

(1.46)

�(t i

, t i�1) = a suitable approximation to (1.28) (1.47)

S i

= a suitable approximation to (1.42) (1.48)

P� i

= �(t i

, t i�1)P i�1�

T(t i

, t i�1) + S i

, P+ 0 = P o

(1.49)

K i

= P� i

HT

i

H i

P� i

HT

i

+R i

��1 (1.50)

X̂ i

= X̂ i

+K i

Y i

� h i

(X̂� i

(1.51)

P+ i

= (I�K i

H i

)P� i

(I�K i

H i

)T +K i

R i

KT

i

(1.52)

1.2. The Multiplicative Extended Kalman Filter

An interesting variation on the EKF is possible in the context of estimating attitude parameters. An attitude correction may be viewed as a small-angle rotation from a frame associated with the previous estimate to a frame associated with a current estimate. In this context, one may use the previous attitude estimate as a linearization reference for a linearized Kalman Filter’s Jacobian matrices, and estimate the small-angle correction as the filter state. After each state update, one performs a rectification of the attitude reference by applying the small-angle correction. Since for many attitude representations, a frame rotation is multiplicative operation, this procedure has become known as the multiplicative EKF. Chapter 8 covers this subject.

CHAPTER 2

The Covariance Matrix

Contributed by J. Russell Carpenter

As Chapter 1 pointed out, the EKF estimate X̂(t) is at best an approximation for E[X(t)|Y], and hence the EKF symbol P(t) is at best an approximation for E[e(t)eT(t)|Y]. In the present Chapter we discuss best practices for maintaining such approximations, and hence-forth we will simply refer to P(t) as the covariance matrix.

2.1. Metrics for Orbit State Covariances

To discuss what makes one covariance approximate better or worse than another, we must be able to compare matrices to one another, and hence we must adopt metrics. For ma-trices that serve as coe�cients, there exist various matrix norms that serve. For covariance matrices, we are usually more interested in measures of the range of possible realizations that could be drawn from the probability distribution characterized by the covariance. The square root of the trace of the covariance is often a reasonable choice, since it is the root sum squared (RSS) formal estimation error. A drawback to the trace for orbital applications is that coordinates and their derivatives typically di↵er by several orders of magnitude, so that for example the RSS position error will dominate the RSS velocity error if the trace is taken over a 6 ⇥ 6 Cartesian position and velocity state error covariance, unless some scaling is introduced. Although arbitrary scalings are possible, we discuss several metrics herein that have been found to be especially suitable to space applications.

Orbit determination is distinguishable from other types of positioning and navigation not only by the use of dynamics suitable to orbiting bodies, but also by a fundamental need to produce states that predict accurately. This need arises because spacecraft operations require accurate predictions for acquisition by communications assets, for planning future activities such as maneuvers and observations, for predicting conjunctions with other space objects, etc. For closed, i.e. elliptical, orbits about most planetary bodies, the two-body potential dominates all other forces by several orders of magnitude. Thus, in most cases, the ability of an orbit estimate to predict accurately is dominated by semi-major axis (SMA) error, �a. This is because SMA error translates into period error through Kepler’s third law, and an error in orbit period translates into a secularly increasing error in position along the orbit track. As Reference [6] shows, the along-track drift per orbit revolution, �s, for an elliptical orbit with eccentricity e is bounded by

�s = �3⇡ r

1 + e

1� e �a from periapse to periapse (2.1)

�s = �3⇡ r

1� e

1 + e �a from apoapse to apoapse (2.2)

2.1. METRICS FOR ORBIT STATE COVARIANCES 11

figures of merit for evaluating orbit determination performance, particularly for relative navigation applications.

In fact, it is easy to show that any function of two (scalar) random variables possesses a similar correlation and balance structure, at least to first order. For example, navigation requirements for atmospheric entry are often stated in terms of flight-path angle error, ��.

Since sin � = ~ r/k~rk · ~v/k~vk, then from geometrical considerations we should expect that �� depends on the component of position error which is in the local horizontal plane, in the direction of the velocity vector, and on the component of velocity error that is normal to both velocity and angular momentum, i.e. binormal to the velocity vector. These are of course the in-plane components of position and velocity that are normal to radius and speed. Thus by using the pair �a and �� as metrics, we can fully characterize the correlation and balance of the in-plane covariance components. The following subsections derive these relationships.

2.1.1. Variance of an Arbitrary Function of Two Random Variables Suppose there exists a random variable z which is a possibly nonlinear function of two other random variables, x and y, such that z = f(x, y) (2.4) and let the joint covariance of x and y be given by

P =

�2 x xy x y xy x y

�2 y

(2.5)

The variance of z is given by �2 z

= E

(z� E[z])2

(2.6) where

E[z] =

Z 1

�1 ⇣ p z

(⇣) d⇣ =

Z 1

�1

Z 1

�1 f(⇠, ⌘) p z

(f(⇠, ⌘)) d⇠ d⌘ (2.7)

Let x̂ = E[x] and ŷ = E[y]. Then E[x� x̂] = 0 and E[y � ŷ] = 0. Expanding f(x, y) around f(x̂, ŷ) in a Taylor series to first order, we find that f(x, y) ⇡ f(x̂, ŷ) + f x

· (x� x̂) + f y

· (y � ŷ) (2.8) where f x and f y are the partials of f with respect to x and y, respectively; so, to first order, ẑ = E[z] = E[f(x, y)] ⇡ f(x̂, ŷ) (2.9)

Now let us similarly expand (z� E[z])2:

(z� E[z])2 = (f(x, y)� f(x̂, ŷ))2 (2.10)

⇡ f2 x

· (x� x̂)2 + 2f x f y

· (x� x̂)(y � ŷ) + f2 y

· (y � ŷ)2 (2.11)

Taking expectations on both sides yields

E

(z� E[z])2

= f2 x

E

(x� x̂)2

+ 2f x f y

E[(x� x̂)(y � ŷ)] + f2 y

E

(y � ŷ)2

(2.12)

�2 z

= f2 x

�2 x

+ 2f x f y xy x y

+ f2 y

�2 y

(2.13)

= FPFT (2.14)

where F = [f x

, f y

]. Since �1 < ⇢ xy

< 1, it is clear that a high negative correlation between x and y will minimize � z for given values of � x and � y

, but if either f x x

>> f y y or f x x

<< f y y

, the impact of the negative correlation will be insignificant. Thus, the only way to simultaneously achieve � z

<< f x x and � z

<< f x y is when ⇢ xy

⇡ �1 and

12 2. THE COVARIANCE MATRIX

f x x

⇡ f y y

, which are the correlation and balance conditions mentioned above, and which occur along the diagonal of Figure 1.

Note also that by defining new variables scaled by their respective partial derivatives, x̃ = xf x and ỹ = yf y

, and correspondingly �̃ x

= f x x and �̃ y

= f y y

, then a normalization of the fashion described above is also possible:

z q

�̃2 x

+ 2⇢ xy x y

+ �̃2 y

(2.15)

2.1.2. Semi-Major Axis Variance To derive a relationship for semi-major axis vari-ance, let us take variations on (2.3), which results in

�a a2

2�r r2

2v�v µ (2.16)

If we replace the variations with deviations of random variables from their expectations, and the non-deviated terms with their expected values, we find that

(a� â) = 2â2

(r � r̂) r̂2 v̂(v � v̂) µ

(2.17) which by squaring and taking expectation yields the following equation for the SMA vari-ance:

�2 a

= 4â4 r̂4 �2 r

+ 2 v̂ µr̂2 rv r v v̂2 µ2 �2 v

(2.18)

For the normalization used in Figure 1, rewrite (2.18) as a

= 2 s r r̂2/â2

◆2

+ 2⇢ rv r r̂2/â2 v µ/(â2v̂) v µ/(â2v̂)

◆2

(2.19) and note that µ/(â2v̂) = n̂v̂ c

/v̂. As mentioned above, normalizing radius and speed standard deviation in this manner permits comparison of data across all points in all closed orbits.

If the orbit is exactly circular, then further simplification of (2.18) is possible. In this case, a = r and v/µ = T p

/2⇡, where T p is the orbit period. Then (2.18) may be rewritten as a

= 2 s

�2 r

+ 2

T p

2⇡ rv r v

T p

2⇡

◆2

�2 v

(2.20)

For orbit determination applications, the state representation most often chosen is a Cartesian inertial state vector, x = [~rT, ~vT]T. Rewriting (2.3) as a(x) = k~rk � k~vk2 µ

◆�1

(2.21) and taking partials yields

@a

@x x̂

= F a

(x̂) = 2â2 r

T r̂3 v

T µ

(2.22) so that

�2 a

= F a

(x̂)P x

FT

a

(x̂) (2.23) where P is the state error covariance.

2.1. METRICS FOR ORBIT STATE COVARIANCES 13

2.1.3. Flight-Path Angle Variance The flight-path angle, �, is the angle between the velocity vector and the local horizontal plane; it is therefore the complement of the angle between the position and velocity vectors, so that

� = arcsin(u r

· u v

) (2.24) where u r r/r and u v v/v. Taking partials with respect to x, we find that

F

(x̂) =

@x x̂ p

1� sin2 �̂ u

T v̂

� sin �̂uT r̂ r̂ u

T r̂

� sin �̂uT v̂ v̂

(2.25) so that �2

= F

(x̂)P x

FT

(x̂) (2.26) which is a form suitable for use in an OD filter estimating a Cartesian inertial state.

For analysis, a simpler form of (2.26) is as follows. Let us define two vectors that are normal to both the position vector and a vector normal to the orbit plane, ~n: the unit in-track vector, u�~ v

= u n

⇥ u r u v

� u r sin � p

1� sin2 � (2.27) which defines a unit vector in the orbit plane that is along the orbit track at apoapsis and periapsis, and the unit bi-normal vector, u b

= u v

⇥ u n u r

� u v sin � p

1� sin2 � (2.28) which defines a unit vector in the orbit plane that is along the position vector at apoapsis and peripasis. Let us next define a composite transformation matrix as follows. Let

M rtn u rI u�~ vI u nI

(2.29) where the subscript I indicates that the specified vector is expressed in an inertial basis.

The unitary matrix MT rtn thus transforms coordinates of vectors in physical space, which are given in the frame I, to a coordinates defined in the basis given by the the radial, transverse, and normal unit vectors. Similarly, define M vnb as

M vnb u vI u nI u bI

(2.30)

Now define the block diagonal transformation matrix M as

M =

M rtn

03x3 03x3 M vnb

(2.31)

Using the matrix M, we can transform the state error covariance, given in inertial coor-dinates, such that its position error covariance is expressed in the “RTN” frame, and its velocity error covariance is in the “VNB” frame. Using the preceding results in (2.26) results in considerable simplification:

�2 u

T t̂

/r̂ u

T b̂

/v̂

MT

03x3 03x3 MT vnb

P

03x3 03x3 M vnb u t̂

/r̂ u b̂

/v̂

(2.32) rt r̂

⌘2 + 2⇢ rtvb rt r̂ vb v̂ vb v̂

⌘2 (2.33) which demonstrates the aforementioned assertion that flight-path angle error depends on the in-track component of position error, and the bi-normal component of velocity error.

Note that (2.33) possesses the desirable feature that the relevant covariance information (in-track position variance and bi-normal velocity variance) is normalized by radius and

2.2. COVARIANCE PROPAGATION 15

For orbits that are very close to circular:

a

= 2 s

�2 r

+ 2

T p

2⇡ rv r v

T p

2⇡

◆2

�2 v

(2.34)

For elliptical orbits:

a

= 2â2 s r̂4 �2 r

+ 2 v̂ µr̂2 rv r v v̂2 µ2 �2 v

(2.35)

For normalization of radius and speed standard deviations across points in closed orbits:

a

= 2 s r r̂2/â2

◆2

+ 2⇢ rv r r̂2/â2 v n̂v̂ c

/v̂ v n̂v̂ c

/v̂

◆2

(2.36)

For entry applications, and as a secondary metric:

r rt r̂

⌘2 + 2⇢ rtvb rt r̂ vb v̂ vb v̂

⌘2 (2.37)

For use in an OD filter that is estimating a Cartesian inertial state vector:

a q

F a

(x̂)P x

FT

a

(x̂), F a

(x̂) = 2â2 r

T r̂3 v

T µ

(2.38) q

F

(x̂)P x

FT

(x̂), F

(x̂) = u

T t̂ r̂ u

T b̂ v̂

(2.39)

2.2. Covariance Propagation

This section discusses best practices for implementing the covariance propagation re-cursion

P� i

= �(t i

, t i�1)P i�1�

T(t i

, t i�1) + S i

, P+ 0 = P o

(2.40)

As Chapter 1 mentions, to achieve this goal we need suitable approximations for the state transition matrix, �(t i

, t i�1), and the process noise covariance, S i

. However, the suitability of a given set of approximations is strongly dependent on specifics of the application. For example, if a set of measurements from which the state is fully observable are available at an interval that is a small fraction of the orbit period, without significant drop-outs, and prediction of the covariance far into the future of the time of availability of the measurements is not required, then simple models such as those to which Chapter 1 alluded have been successfully employed. Selection of appropriate covariance propagation approximations also depends strongly on the choice of state representation, which is the subject of Chapter 6.

Therefore, this section will discuss the merits of some of the more common and generally applicable approaches, in the context of orbit determination.

2.2.1. Matrix Ricatti Equation Many textbooks on Kalman filtering derive (2.40) as the solution of a matrix Ricatti equation; using the notation of Chapter 1, this takes the following form:

Ṗ(t) = A(t)P(t) +P(t)AT(t) +Q(t) (2.41)

Use of (2.41) would seem to avoid the need to perform the integrations required to compute the state transition matrix (STM) and process noise covariance (PNC). In orbit determina-tion practice however, (2.40) has been found to be more numerically stable and also, despite

16 2. THE COVARIANCE MATRIX

the need to compute or approximate the state transition and process noise integrals, more e�cient than (2.41).

2.2.2. State Transition Matrix A common approach in ground-based OD, especially in the batch least squares context, is to simultaneously integrate the STM along with the state vector, Ẋ(t) = f(X(t), t), X(t o

) = X o

(2.42)

�̇(t, t o

) = A(t)�(t, t o

), �(t o

, t o

) = I (2.43)

When coupled with a good numerical integration algorithm, this method has excellent fidelity, which is rarely necessary in onboard OD applications. Lear [40] studied a number of practical methods for computing the STM in the onboard OD context. As his report is not widely available, we will summarize some key findings here.

Lear’s approach was to compare various orders of truncated Taylor series and Runge- Kutta approximations to the solution of (2.43). He used these STM approximations to propagate an initially diagonal covariance for one revolution in a two-body circular orbit around a point mass with the GM of Earth. By comparing these results to those he obtained using an analytic STM, Lear could compute the maximum step size that would result in a given relative accuracy for radius and speed formal standard deviations. Table 1 lists a few of Lear’s results. Notably, Method H has the desirable feature that simply saving the value

Description Method Max. Step [sec] A. 1st-order Taylor I+A i

�t 0.125 B. 2nd-order Taylor, ignoring Ȧ I+A i

�t+A2 i

�t2/2 1.0 C. 2nd-order Taylor I+A i

�t+ (Ȧ i

+A2 i

)�t2/2 16 F. 1st-order Runge-Kutta I+A i+.5�t 0.14 G. 2nd-order Runge-Kutta, with one evaluation of A I+A i+.5�t+A2 i+.5�t2/2 14

H. 2nd-order Runge-Kutta, with two evaluations of A

I+(A i

+A i+1)�t+A i

A i+1�t2/2 16

Table 1. A few of Lear’s STM Comparison Results, for 1% relative error.

of the state Jacobian from the previous propagation step allows for more than an order of magnitude increase in allowable time step, with essentially the same computational burden as Method B. If it is not too burdensome to compute A at the midpoint of the propagation step, then Method G o↵ers nearly equivalent performance without the need to retain the previous value of A. For higher-rate propagations, Method B o↵ers far more accuracy than Method A with only a small additional computational burden. While Method A appears to be a poor choice for many applications, it does play a central role in some useful approximations to the process noise covariance, as the sequel shows.

2.2.3. Process Noise Covariance As stated in Chapter 1, nearly all practical meth-ods for computing the process noise covariance assume that E[w(t)wT(⌧)] = Q(t)�(t�⌧), so that (1.29) simplifies to a single integral for the process noise covariance1, given by (1.42), 1A notable exception is the work of Wright [76], which describes a correlated process noise model that is intended to account for gravity modeling errors in a “physically realistic” manner. Although this method

2.2. COVARIANCE PROPAGATION 17

and repeated here:

S i

Z t i t i�1

�(t i

, ⌧)B(⌧)Q(⌧)BT(⌧)�T(t i

, ⌧) d⌧ (2.44)

Tapley, Schutz, and Born [69] describe two approximations to the portion of (2.44) which corresponds to position and velocity state noise, which have proven useful in…