Tài liệu Lọc Kalman - lý thuyết và thực hành bằng cách sử dụng MATLAB (P7) ppt

A dual-state ®lter implementation can be used to analyze the expected performance ofsimpli®ed Kalman ®lters, based on simplifying the dynamic system modeland=or measurement model.. The o

Practical Considerations

``The time has come,'' the Walrus said,

``To talk of many things:

Of shoesÐand shipsÐand sealing waxÐ

Of cabbagesÐand kingsÐAnd why the sea is boiling hotÐAnd whether pigs have wings.''From ``The Walrus and the Carpenter,'' in Through the Looking Glass, 1872

Lewis Carroll [Charles Lutwidge Dodgson] (1832±1898)

7.1 CHAPTER FOCUS

The discussion turns now to what might be called Kalman ®lter engineering, which

is that body of applicable knowledge that has evolved through practical experience

in the use and misuse of the Kalman ®lter The material of the previous two chapters(extended Kalman ®ltering and square-root ®ltering) has also evolved in this wayand is part of the same general subject Here, however, the discussion includes manymore matters of practice than nonlinearities and ®nite-precision arithmetic.7.1.1 Main Points to Be Covered

1 Roundoff errors are not the only causes for the failure of the Kalman ®lter toachieve its theoretical performance There are diagnostic methods for identi-fying causes and remedies for other common patterns of misbehavior

2 Pre®ltering to reduce computational requirements If the dynamics of themeasured variables are ``slow'' relative to the sampling rate, then a simplepre®lter can reduce the overall computational requirements without sacri®cingperformance

270

Mohinder S Grewal, Angus P Andrews Copyright # 2001 John Wiley & Sons, Inc ISBNs: 0-471-39254-5 (Hardback); 0-471-26638-8 (Electronic)

3 Detection and rejection of anomalous sensor data The inverse of the matrix

z H ^x and may be used to test for exogenous measurement errors, such asthose resulting from sensor or transmission malfunctions

4 Statistical design of sensor and estimation systems The covariance equations

of the Kalman ®lter provide an analytical basis for the predictive design ofsystems to estimate the state of dynamic systems They may also be used toobtain suboptimal (but feasible) observation scheduling

5 Testing for asymptotic stability The relative robustness of the Kalman ®lteragainst minor modeling errors is due, in part, to the asymptotic stability of theRiccati equations de®ning performance

6 Model simpli®cations to reduce computational requirements A dual-state

®lter implementation can be used to analyze the expected performance ofsimpli®ed Kalman ®lters, based on simplifying the dynamic system modeland=or measurement model These analyses characterize the trade-offsbetween performance and computational requirements

7 Memory and throughput requirements These computational requirements arerepresented as functions of ``problem parameters'' such as the dimensions ofstate and measurement vectors

8 Of¯ine processing to reduce on-line computational requirements Except inextended (nonlinear) Kalman ®ltering, the gain computations do not dependupon the real-time data Therefore, they can be precomputed to reduce thereal-time computational load

9 Application to aided inertial navigation, in which the power of Kalman

®ltering is demonstrated on a realistic problem (see also [22])

7.2 DETECTING AND CORRECTING ANOMALOUS BEHAVIOR7.2.1 Convergence, Divergence, and ``Failure to Converge''

of real vectors Zkis said to converge to a limit Z1if, for every e > 0, for some n, for all

k > n, the norm of the differences kZk Z1k < e Let us use the expressions

lim

to represent convergence One vector sequence is said to converge to another vectorsequence if their differences converge to the zero vector, and a sequence is said toconverge1 if, for every e > 0, for some integer n, for all k; ` > n, kZk Z`k < e

1 Such sequences are called Cauchy sequences, after Augustin Louis Cauchy (1789±1857).

Divergence is de®ned as convergence to 1: for every e > 0, for some integer n, forall k > n, jZkj > e In that case, kZkk is said to grow without bound.

Nonconvergence This is a more common issue in the performance of Kalman

®lters than strict divergence That is, the ®lter fails because it does not converge tothe desired limit, although it does not necessarily diverge

Dynamic and Stochastic Variables Subject to Convergence or gence The operation of a Kalman ®lter involves the following sequences that may

Diver-or may not converge Diver-or diverge:

xk; the sequence of actual state values;

E xkxT

k

; the mean-squared state;

^xk; the estimated state;

~xk… † ˆ ^xk… † xk; the a priori estimation error;

~xk…‡† ˆ ^xk…‡† xk; the a posteriori estimation error;

Pk… † ; the covariance of a priori estimation errors;

Pk…‡† ; the covariance of a posteriori estimation errors

One may also be interested in whether or not the sequences fPk… †g and fPk…‡†gcomputed from the Riccati equations converge to the corresponding true covariances

of estimation error

7.2.2 Use of Riccati Equation to Predict Behavior

The covariance matrix of estimation uncertainty characterizes the theoreticalperformance of the Kalman ®lter It is computed as an ancillary variable in theKalman ®lter as the solution of a matrix Riccati equation with the given initialconditions It is also useful for predicting performance If its characteristic values aregrowing without bound, then the theoretical performance of the Kalman ®lter is said

to be diverging This can happen if the system state is unstable and unobservable, forexample This type of divergence is detectable by solving the Riccati equation tocompute the covariance matrix

The Riccati equation is not always well conditioned for numerical solution andone may need to use the more numerically stable methods of Chapter 6 to obtainreasonable results One can, for example, use eigenvalue±eigenvector decomposition

of solutions to test their characteristic roots (they should be positive) and condition

reciprocal of the unit roundoff error in computer precision) are considered probablecause for concern and reason to use square-root methods

7.2.3 Testing for Unpredictable Behavior

Not all ®lter divergence is predictable from the Riccati equation solution Sometimesthe actual performance does not agree with theoretical performance

One cannot measure estimation error directly, except in simulations, so one must

®nd other means to check on estimation accuracy Whenever the estimation error isdeemed to differ signi®cantly from its expected value (as computed by the Riccatiequation), the ®lter is said to diverge from its predicted performance We will nowconsider how one might go about detecting divergence

Examples of typical behaviors of Kalman ®lters are shown in Figure 7.1, which is

a multiplot of the estimation errors on 10 different simulations of a ®lter mentation with independent pseudorandom-error sequences Note that each time the

imple-®lter is run, different estimation errors ~x…t† result, even with the same initialcondition ^x…0†: Also note that at any particular time the average estimation error(across the ensemble of simulations) is approximately zero,

1N

PN iˆ1‰^xi…tk† x…tk†Š  Ei ^xi…tk† x…tk†ˆ 0; …7:1†where N is the number of simulation runs and ^xi…tk† x…tk† is the estimation error attime tk on the ith simulation run

Monte Carlo analysis of Kalman ®lter performance uses many such runs to testthat the ensemble mean estimation error is unbiased (i.e., has effectively zero mean)and that its ensemble covariance is in close agreement with the theoretical valuecomputed as a solution of the Riccati equation

Convergence of Suboptimal Filters In the suboptimal ®lters discussed inSection 7.5, the estimates can be biased Therefore, in the analysis of suboptimal

®lters, the behavior of P…t† is not suf®cient to de®ne convergence A suboptimal

®lter is said to converge if the covariance matrices converge,

lim

Fig 7.1 Dispersion of multiple runs.

and the asymptotic estimation error is unbiased,

lim

Example 7.1: Some typical behavior patterns of suboptimal ®lter convergence aredepicted by the plots of P…t† in Figure 7.2a, and characteristics of systems with thesesymptoms are given here as examples

Case A: Let a scalar continuous system equation be given by

in which the system is unstable, or

in which the system has driving noise and is unstable

Case B: The system has constant steady-state uncertainty:

lim

Case C: The system is stable and has no driving noise:

Figure 7.2b for the following system characteristics:

Case A: Effects of system driving noise and measurement noise are large relative

to P0…t† (initial uncertainty)

Case B: P0ˆ P1 (Wiener ®lter)

Case C: Effects of system driving noise and measurement noise are small relative

to P0…t†:

Fig 7.2 Asymptotic behaviors of estimation uncertainties.

Example 7.3: Continuous System with Discrete Measurements A scalar ple of a behavior pattern of the covariance propagation equation …Pk… †; _P…t†† andcovariance update equation Pk…‡†,

exam-_x…t† ˆ Fx…t† ‡ w…t†; F < 0;

z…t† ˆ x…t† ‡ v…t†;

is shown in Figure 7.2c

The following features may be observed in the behavior of P…t†:

1 Processing the measurement tends to reduce P:

2 Process noise covariance …Q† tends to increase P:

3 Damping in a stable system tends to reduce P:

4 Unstable system dynamics …F > 0† tend to increase P:

5 With white Gaussian measurement noise, the time between samples …T† can bereduced to decrease P:

The behavior of P represents a composite of all these effects (1±5) as shown inFigure 7.2c

Causes of Predicted Nonconvergence Nonconvergence of P predicted bythe Riccati equation can be caused by

1 ``natural behavior'' of the dynamic equations or

2 nonobservability with the given measurements

The following examples illustrate these behavioral patterns

Example 7.4: The ``natural behavior'' for P in some cases is for

_P ˆ Q PR2:The solution is

t!1P…t† ˆ a > 0; a ®nite, but nonzero, limit (See Figure 7.3a.)

2 This is no cause for alarm, and there is no need to remedy the situation if theasymptotic mean-squared uncertainty is tolerable If it is not tolerable, then theremedy must be found in the hardware (e.g., by attention to the physicalsources of R or QÐor both) and not in software

11 11

Fig 7.3 Behavior patterns of P.

Example 7.5: Divergence Due to ``Structural'' Unobservability The ®lter issaid to diverge at in®nity if its limit is unbounded:

0 < lim

is actually bounded away from zero uncertainty

Parameter estimation model for continuous time Consider the two-dimensionalparameter estimation problem

``upstairs'' and make them superscripts (This approach only works here because theproblem is linear Therefore, one does not need superscripts to indicate powers of thecomponents.) For these purposes, let xi

kdenote the ith component of the state vector

at time tk The continuous form of the parameter estimation problem can then be

``discretized'' to a model for a discrete Kalman ®lter (for which the state transitionmatrix is the identity matrix; see Section 4.2):

1 the a priori information in ^x0 and P0 and

2 the measurement sequence zkˆ x1‡ x2‡ vk for k ˆ 1; 2; 3; :

In this case, the best the optimal ®lter can do with the measurements is to ``averageout'' the effects of the noise sequence v1; ; vk: One might expect that an in®nite

that is,

g

Estimation uncertainty from a single noise-free measurement By using thediscrete ®lter equations with one stage of estimation on the measurement z1, onecan obtain the gain in the form

37

1 As a consequence, the ®lter willalmost never get the right answer! This is a fundamental characteristic of the

problem, however, and not attributable to the design of the ®lter There are twounknowns (x1

1 and x2

1) and one constraint:

z1ˆ x1

1‡ x2 1

s2

2…0†

s2…0†ˆ C ˆ

x2 1

because s2

1…0† and s2

2…0† are nonnegative numbers

Likelihood of serendipitous design For the ®lter to obtain the right answer, itwould be necessary that

1 x1 and x2 have the same sign and

2 it is known that their ratio C ˆ x2=x1

Since both of these conditions are rarely satis®ed, the ®lter estimates would rarely becorrect

What can be done about it The following methods can be used to detectnonconvergence due to this type of structural unobservability:

 Test the system for observability using the ``observability theorems'' of Section2.5

high correlation coef®cients (e.g., jrj > 0:9)

 Perform eigenvalue±eigenvector decomposition of P to test for negativecharacteristic values or a large condition number (This is a better test thancorrelation coef®cients to detect unobservability.)

 Test the ®lter on a system simulator with noise-free outputs (measurements).Remedies for this problem include

 de®ning x  x1‡ x2 as the only state variable to be estimated

Example 7.7: Unobservability Caused by Poor Choice of Sampling Rate Theproblem in Example 7.6 might be solved by using an additional measurementÐor

by using a measurement with a time-varying sensitivity matrix Next, consider whatcan go wrong even with time-varying measurement sensitivities if the sampling rate

is chosen badly For that purpose, consider the problem of estimating unknownparameters (constant states) with both constant and sinusoidal measurement sensi-tivity matrix components:

" #

‡ vk;

where T is the intersample interval

What happens when Murphy's law takes effect With the choice of intersampling

matrix become equal and constant:

7.4) With this choice of sampling times, the system and ®lter behave as in theprevious example.

Methods for detecting and correcting unobservability include those given inExample 7.6 plus the more obvious remedy of changing the sampling interval T toobtain observability, for example,

 The initial estimate is badly chosen, for example,

Practical limitations of recovery Often, in practice, the recovery is not adequate

in ®nite time The interval …0; T† of measurement availability is ®xed and may be too

short to allow suf®cient recovery (see Figure 7.6) The normal behavior of the gainmatrix K may be too rapidly converging toward its steady-state value of K ˆ 0 (SeeFigure 7.7.)

Remedies for ``heading off'' bad data:

 Inspection of P…t† and K…t† is useless, because they are not affected by data

 Inspection of the state estimates ^x…t† for sudden jumps (after a bad ment has already been used by the ®lter) is sometimes used, but it still leavesthe problem of undoing the damage to the estimate after it has been done

entries (before bad measurement is processed by the ®lter) is much moreuseful, because the discrepancies can be interpreted probabilistically, and thedata can be discarded before it has spoiled the estimate (see Section 7.3).The best remedy for this problem is to implement a ``bad data detector'' to reject thebad data before it contaminates the estimate If this is to be done in real time, it issometimes useful to save the bad data for off-line examination by an ``exceptionhandler'' (often a human, but sometimes a second-level data analysis program) tolocate and remedy the causes of the bad data that are occurring

Arti®cially increasing process noise covariance to improve bad data recovery Ifbad data are detected after they have been used, one can keep the ®lter ``alive'' (topay more attention to subsequent data) by increasing the process noise covariance Q

in the system model assumed by the ®lter Ideally, the new process noise covarianceshould re¯ect the actual measurement error covariance, including the bad data aswell as other random noise

Example 7.9: Nonconvergence Due to Numerical Problems This is sometimesdetected by observing impossible Pkbehavior The terms on the main diagonal of Pkmay become negative, or larger, immediately after a measurement is processed thanimmediately before, that is, s…‡† > s… † A less obvious (but detectable) failuremode is for the characteristic values of P to become negative This can be detected

by eigenvalue±eigenvector decomposition of P Other means of detection includeusing simulation (with known states) to compare estimation errors with theirestimated covariances One can also use double precision in place of single precision

to detect differences due to precision Causes of numerical problems can sometimes

be traced to inadequate wordlength (precision) of the host computer These problemstend to become worse with larger numbers of state variables

Remedies for numerical problems These problems have been treated by many

``brute-force'' methods, such as using higher precision (e.g., double instead ofsingle) One can try reducing the number of states by merging or eliminatingunobservable states, eliminating states representing ``small effects,'' or using othersuboptimal ®lter techniques such as decomposition into lower dimensional statespaces

Possible remedies include the use of more numerically stable methods (to obtainmore computational accuracy with the same computer precision) and the use ofhigher precision The latter approach (higher precision) will increase the executiontime but will generally require less reprogramming effort One can sometimes use abetter algorithm to improve the accuracy of matrix inverse HPHT‡ R
1(e.g., theCholesky decomposition method shown in Chapter 6), or eliminate the inversealtogether by diagonalizing R and processing the measurements sequentially (alsoshown in Chapter 6)

7.2.4 Effects Due to Mismodeling

Lying to the Filter The Kalman gain and the covariance matrix P are correct ifthe models used in computing them are correct With mismodeling, the P matrix can

be erroneous and of little use in detecting nonconvergence, or P can even converge

to zero while the state estimation error ~x is actually diverging (It happens.)The problems that result from bad initial estimates of x or P have already beenaddressed There are four other types that will now be addressed:

1 unmodeled state variables of the dynamic system,

2 unmodeled process noise,

3 errors in the dynamic coef®cients or state transition matrix, and

4 overlooked nonlinearities

Example 7.10: Nonconvergence Caused by Unmodeled State Variables der the following example:

Consi-Real World (Creeping State) Kalman Filter Model (Constant State)

Detecting unmodeled state dynamics by Fourier analysis of the ®lter innovations

It is dif®cult to diagnose unmodeled state variables unless all other causes fornonconvergence have been ruled out If there is high con®dence in the model beingused, then simulation can be used to rule out any of the other causes above Oncethese other causes have been eliminated, Fourier analysis of the ®lter innovations (theprediction errors fzk Hk^xkg) can be useful for spotting characteristic frequencies ofthe unmodeled state dynamics If the ®lter is modeled properly, then the innovationsshould be uncorrelated, having a power spectral density (PSD) that is essentially ¯at.Persistent peaks in the PSD would indicate the characteristic frequencies ofunmodeled effects These peaks can be at zero frequency, which would indicate abias error in the innovations

Remedies for unmodeled state variables The best cure for nonconvergencecaused by unmodeled states is to correct the model, but this not always easy to

do As an ad hoc ®x, additional ``®ctitious'' process noise can be added to the systemmodel assumed by the Kalman ®lter

Example 7.11: Adding ``Fictitious'' Process Noise to the Kalman FilterModel Continuing with the continuous-time problem of Example 7.10, considerthe alternative Kalman ®lter model

transforming the general Riccati differential equation

to a scalar equation with steady-state solution (to _P ˆ 0)

P…1† ˆpRQ:The steady-state Kalman gain



QR

This type of behavior is shown in Figure 7.9b.

If the steady-state bias in the estimation error is unsatisfactory with the approach

in Example 7.11, one can go one step further by adding another state variable and

®ctitious process noise to the system model assumed by the Kalman ®lter.Example 7.12: Effects of Adding States and Process Noise to the Kalman FilterModel Suppose that the model of Example 7.11 was modi®ed to the following

Real World_x1ˆ 0_x2ˆ x1



2

p QR

4

r

266

37

K2…1†

;…7:44†

and these can be Laplace transformed to yield

In component form, this becomes:

This type of behavior is shown in Figure 7.10b

Example 7.13: Statistical Modeling Errors Due to Unmodeled System DrivingNoise With the models

…7:48†Since x1is not constant (due to the driving noise w), the state estimates will notconverge to the states, as illustrated in the simulated case plotted in Figure 7.11 This

®gure shows the behavior of the Kalman gain K1, the estimated state component ^x1,and the ``true'' state component x1in a discrete-time simulation of the above model.Because the assumed process noise is zero, the Kalman ®lter gain K1 converges tozero Because the gain converges to zero, the ®lter is unable to track the errant statevector component x1, a random-walk process Because the ®lter is unable to track thetrue state, the innovations (the difference between the predicted measurement and theactual measurement) continue to grow without bound Even though the gains are

converging to zero, the product of the gain and the innovations (used in updating thestate estimate) can be signi®cant.

In this particular example, s2

1…t† ˆ s2

1…0† ‡ s2

wt That is, the variance of thesystem state itself diverges As in the case of unmodeled states, the innovationsvector ‰z H ^xŠ will show effects of the ``missing'' system driving noise

Example 7.14: Parametric Modeling Errors Having the wrong parameters in thesystem dynamic coef®cients F, state transition matrix F, or output matrix H can anddoes bring about nonconvergence of the ®lter This type of nonconvergence can bedemonstrated by an example with the wrong period of a sinusoid in the outputmatrix:

Real World

_x1_x2

No a priori information exists

Kalman Filter Model_x1

is the performance index being minimized.3 Its gradient

where p is the period of the sinusoid For simplicity, let us choose the sampling time

as T ˆ Np, an integer multiple of the period, so that

1 …Np

0 sin…ot†z…t† dt…Np

0 cos…ot†z…t† dt

266

37

2Np

…Np

0 z…t† sin ot dt2

Np

…Np

0 z…t† cos ot dt

266

37

Concentrating on the ®rst component ^x1; one can obtain its solution as

^x1ˆ Np2 sin…o O†t2…o O† sin…o ‡ O†t2…o ‡ O†

tˆNptˆ0

By setting v ˆ 0 (ignoring the estimation error due to measurement noise), oneobtains the result

^x1ˆNp2 sin…o O†Np2…o O† sin…o ‡ O†Np2…o ‡ O†

sin…o O†Np

Np2

sin xx

... class="page_container" data-page="26">

1 They allow a discrete Kalman ®lter to be used on a continuous system without

``throwing away'''' information For example, the integrate-and-hold pre®ltershown... ~x3…0†:

Nonlinearities in the real-world system also cause nonconvergence or even gence of the Kalman estimates

diver-7.2.5 Analysis and Repair of Covariance Matrices

Covariance... willprovide suboptimal estimates These are

1 Kalman ®lters (linear optimal estimate),

2 linearized Kalman ®lters, and

3 extended Kalman ®lters

Even if there is good reason