Optimal State Estimation

By Dan Simon

Chapter 9.4 - Fixed-Interval Smoothing

9.4 FIXED-INTERVAL SMOOTHING

Suppose we have measurements for a fixed time interval. In fixed-interval smoothing we seek an estimate of the state at some of the interior points of the time interval. During the smoothing process we do not obtain any new measurements. Section 9.4.1 discusses the forward-backward approach to smoothing, which is perhaps the most straightforward smoothing algorithm. Section 9.4.2 discusses the RTS smoother, which is conceptually more difficult but is computationally cheaper than forward-backward smoothing.

9.4.1 Forward-backward smoothing

Suppose we want to estimate the state x_mbased on measurements from k = 1 to k = N, where N > m. The forward-backward approach to smoothing obtains two estimates of x_m. The first estimate, _f, is based on the standard Kalman filter that operates from k = 1 to k = m. The second estimate, _b, is based on a Kalman filter that runs backward in time from k = N back to k = m. The forward-backward approach to smoothing combines the two estimates to form an optimal smoothed estimate. This approach was first suggested in [Pra69].

Suppose that we combine a forward estimate _f of the state and a backward estimate _bof the state to get a smoothed estimate of x as follows:

where K_f and K_b are constant matrix coefficients to be determined. Note that _f and _bare both unbiased since they are both outputs from Kalman filters. Therefore, if x is to be unbiased, we require K_f+ K_b= I (see Problem 9.9). This gives

The covariance of the estimate can then be found as

where e_f = x - x_f , e_b = x - x_b, and we have used the fact that = 0. The estimates _f and _b are both unbiased, and e_f and e_bare independent (since they depend on separate sets of measurements). We can minimize the trace of P with respect to K_f using results from Equation (1.66) and Problem 1.4:

where P_f = is the covariance of the forward estimate, and P_b = is the covariance of the backward estimate. Setting this equal to zero to find the optimal value of K_fgives

The inverse of (P_f + P_b) always exists since both covariance matrices are positive definite. We can substitute this result into Equation (9.57) to find the covariance of the fixed-interval smoother as follows:

Using the identity (A + B) ^-1 = B ^{- 1}(AB^-1 + I)^-1(see Problem 9.2), we can write the above equation as

Multiplying out the first term, and again using the identity (A + B) ^-1 = B ^{- 1}(AB^-1 + I)^-1on the last two terms, results in

From the matrix inversion lemma of Equation (1.39) we see that

Substituting this into Equation (9.62) gives

These results form the basis for the fixed-interval smoothing problem. The system model is given as

Suppose we want a smoothed estimate at time index m. First we run the forward Kalman filter normally, using measurements up to and including time m.

Initialize the forward filter as follows:

For k = 1, …, m, perform the following:

At this point we have a forward estimate for x_m, along with its covariance. These quantities are obtained using measurements up to and including time m.

The backward filter needs to run backward in time, starting at the final time index N. Since the forward and backward estimates must be independent, none of the information that was used in the forward filter is allowed to be used in the backward filter. Therefore, must be infinite:

We are using the minus superscript on to indicate the backward covariance at time N before the measurement at time N is processed. (Recall that the filtering is performed backward in time.) So will be updated to obtain after the measurement at time N is processed. Then it will be extrapolated backward in time to obtain , and so on.

Now the question arises how to initialize the backward state estimate at the final time k = N. We can solve this problem by introducing the new variable

A minus or plus superscript can be added on all the quantities in the above equation to indicate values before or after the measurement at time k is taken into account. Since = ∞ it follows that

The infinite boundary condition on means that we cannot run the standard Kalman filter backward in time because we have to begin with an infinite covariance. Instead we run the information filter from Section 6.2 backward in time. This can be done by writing the system of Equation (9.65) as

Note that should always exist if it comes from a real system, because F_k comes from a matrix exponential that is always invertible (see Sections 1.2 and 1.4). The backward information filter can be written as follows.

Initialize the filter with = 0.

For k = N, N-1, … , perform the following:

The first form for above requires the inversion of . Consider the first time step for the backward filter (i.e., at k = N). The information matrix is initialized to zero, and then the first time through the above loop we set = . If there are fewer measurements than states, will always be singular and, therefore, will be singular at k = N. Therefore, the first form given above for will not be computable. In practice we can get around this by initializing to a small nonzero matrix instead of zero.

The third form for above has its own problems. It does not require the inversion of , but it does require the inversion of Q_k-₁. So the third form of is not computable unless Q_k-₁ is nonsingular. Again, in practice we can get around this by making a small modification to Q_k-₁ so that it is numerically nonsingular.

Since we need to update s_k = _bk_bk instead of _bk (because of initialization issues) as defined in Equation (9.69), we rewrite the update equations for the state estimate as follows:

Now note from Equation (6.33) that we can write = + , and K_bk = . Substituting these expressions into the above equation for gives

We combine this with Equation (9.72) to write the backward information filter as follows.

Initialize the filter as follows:

For k = N, N - 1, …, m + 1, perform the following:

Perform one final time update to obtain the backward estimate of x_m:

Now we have the backward estimate and its covariance . These quantities are obtained from measurements m + 1, m + 2, …, N.

After we obtain the backward quantities as outlined above, we combine them with the forward quantities from Equation (9.67) to obtain the final state estimate and covariance:

We can obtain an alternative equation for _m by manipulating the above equations. If we substitute for K_f in the above expression for _mthen we obtain

Using the matrix inversion lemma on the rightmost inverse in the above equation and performing some other manipulations gives

where we have relied on the identity(A + B) ^-1 = B ^{- 1}(AB^-1 + I)^-1(see Problem 9.2). The coefficients of and in the above equation both have a common factor which can be written as follows:

Therefore, using Equation (9.78), we can write Equation (9.80) as

Figure 9.9 illustrates how the forward-backward smoother works.

Figure 9.9 This figure illustrates the concept of the forward-backward smoother. The forward filter is run to obtain a posteriori estimates and covariances up to time m. Then the backward filter is run to obtain a priori estimates and covariances back to time m (i.e., a priori from a reversed time perspective). Then the forward and backward estimates and covariances at time m are combined to obtain the final estimate _m and covariance P_m.

■ EXAMPLE 9.3

In this example we consider the same problem given in Example 9.1. Suppose that we want to estimate the position and velocity of the vehicle at t = 5 seconds. We have measurements every 0.1 seconds for a total of 10 seconds. The standard deviation of the measurement noise is 10, and the standard deviation of the acceleration noise is 10. Figure 9.10 shows the trace of the covariance of the estimation of the forward filter as it runs from t = 0 to t = 5, the backward filter as it runs from t = 10 back to t = 5, and the smoothed estimate at t = 5. The forward and backward filters both converge to the same steady-state value, even though the forward filter was initialized to a covariance of 20 for both the position and velocity estimation errors, and the backward filter was initialized to an infinite covariance. The smoothed filter has a covariance of about 7.6, which shows the dramatic improvement that can be obtained in estimation accuracy when smoothing is used.

Figure 9.10 This shows the trace of the estimation-error covariance for Example 9.3. The forward filter runs from t = 0 to t = 5, the backward filter runs from t = 10 to t = 5, and the trace of the covariance of the smoothed estimate is shown at t = 5.

9.4.2 RTS smoothing

Several other forms of the fixed-interval smoother have been obtained. One of the most common is the smoother that was presented by Rauch, Tung, and Striebet, usually called the RTS smoother [Rau65]. The RTS smoother is more computationally efficient than the smoother presented in the previous section because we do not need to directly compute the backward estimate or covariance in order to get the smoothed estimate and covariance. In order to obtain the RTS smoother, we will first look at the smoothed covariance given in Equation (9.78) and obtain an equivalent expression that does not use P_bm. Then we will look at the snoothed estimate given in Equation (9.78), which uses the gain K_f, which depends on P_bm, and obtain an equivalent expression that does not use P_bm or _bm.

9.4.2.1 RTS covariance updateFirst consider the smoothed covariance given in Equation (9.78). This can be written as

where the second expression comes from an application of the matrix inversion lemma to the first expression (see Problem 9.3). Prom Equation (9.72) we see that

Substituting this into the expression (+ ) ^-1 gives the following:

From Equations (6.26) and (9.76) recall that

We can combine these two equations to obtain

Substituting this into Equation (9.78) gives

Substituting this into Equation (9.85) gives

where the last equality comes from an application of the matrix inversion lemma. Substituting this expression into Equation (9.83) gives

where the smoother gain K_mis given as

The covariance update equation for P_m is not a function of the backward covariance. The smoother covariance P_mcan be solved by using only the forward covariance P_fm, which reduces the computational effort (compared to the algorithm presented in Section 9.4.1).

9.4.2.2 RTS state estimate updateNext we consider the smoothed estimate _mgiven in Equation (9.78). We will find an equivalent expression that does not use P_bm or _bm . In order to do this we will first need to establish a few lemmas.

Lemma 1

Proof: From Equation (9.67) we see that

Rearranging this equation gives

Premultiplying both sides by and postmultiplying both sides by gives thedesired result.
QED

Lemma 2The a posteriori covariance of the backward filter satisfies the equation

Proof: From Equation (9.78) we obtain

QED

Lemma 3The covariances of the forward and backward filters satisfy the equation

Proof: From Equation (9.67) and (9.72) we see that

Adding these two equations and rearranging gives

QED

Lemma 4The smoothed estimate _k can be written as

Proof: From Equations (9.69) and (9.82) we have

From Equation (9.76) we see that

Substitute this expression for , and the expression for from Equation (9.67),into Equation (9.101) to obtain

Now substitute for K_fk [from Equation (9.67)] in the above equationto obtain

QED

Lemma 5

Proof: Recall from Equations (6.26) and (9.72) that

Combining these two equations gives

where we have used Equation (9.78) in the above derivation. Substitute this expression for into Equation (9.97) to obtain

Invert both sides to obtain

Now apply the matrix inversion lemma to the term in the above equation. This results in

QED

With the above lemmas we now have the tools that we need to obtain an alternate expression for the smoothed estimate. Starting with the expression for in Equation (9.76), and substituting the expression for from Equation (9.72) gives

Rearranging this equation gives

Multiplying out this equation, and premultiplying both sides by , gives

Substituting for K_k-₁ from Equation (9.92) gives

Substituting in this expression for from Equation (9.95) gives

Substituting for + from Equation (9.97) on both sides of this expression gives

Premultiplying both sides by gives

Substituting Equation (9.105) for

Now from Equation (9.105) we see that

So we can add the two sides of this equation to the two sides of Equation (9.118) to get

Now use Equation (9.100) to substitute for P_k in the above equation and obtain

Rearrange this equation to obtain

From Equation (9.106) we see that . Also note that part of the coefficient of on the left side of the above equation can be expressed as

From Equation (9.67) we see that = . Therefore Equation (9.122) can be written as

Now substitute for P_kfrom Equation (9.90) and use Equation (9.91) in the above equation to obtain

Premultiplying both sides by gives

Now use Equation (9.91) to notice that the coefficient of on the left side of the above equation can be written as

Using Equation (9.90) to substitute for allows us to write the above expression as

Since this is the coefficient of in Equation (9.126), we can write that equation as

Now from Equations (9.78) and (9.82) we see that

From this we see that

Rewriting the above equation with the time subscripts (k - 1) and then substituting for the left side of Equation (9.129) gives

from which we can write

This gives the smoothed estimate _k without needing to explicitly calculate the backward estimate. The RTS smoother is implemented by first running the standard Kalman filter of Equation (9.67) forward in time to the final time, and then implementing Equations (9.90), (9.91), and (9.133) backward in time. The RTS smoother can be summarized as follows.

The RTS smoother