Optimal observer design

Statement of the problem

Here is the deterministic, finite-horizon, continuous-time Kalman Filter (KF) problem — i.e., the optimal control problem that one would solve to produce an optimal observer:

The interpretation of this problem is as follows. The current time is . You have taken measurements over the time interval . You are looking for noise and disturbance over this same time interval and for an estimate of the current state that would best explain these measurements.

The matrices , , and are parameters that can be used to trade off noise (the difference between the measurements and what you expect them to be) with disturbance (the difference between the time derivative of the state and what you expect it to be). These matrices have to be symmetric, have to be the right size, and also have to satisfy the following conditions in order for the KF problem to have a solution:

Just as with the LQR problem, this notation means that and are positive semidefinite and that is positive definite (see wikipedia).

By plugging in the expression for that appears in the constraint, this optimal control problem can be rewritten as

It is an optimal control problem, just like LQR — if you define

then you see that this problem has the general form

There are four differences between this form and the one we saw when solving the LQR problem:

• The “input” in this problem is , not .

• The “current time” is and not .

• The final state — i.e., the state at the current time — is not given. Indeed, the point here is to choose a final state that best explains and .

• The functions , , and vary with time (because they have parameters in them — and — that are functions of time).

Because of these four differences, the HJB equation for a problem of this form is

Note the change in sign of both the first term outside the minimum and the first term inside the minimum — this is because we are effectively solving an optimal control problem in which time flows backward (from the current time to the initial time , instead of from the current time to the final time ). It is possible to derive this form of the HJB equation in exactly the same way as it was done in the notes on LQR.

Solution to the problem

As usual, our first step is to find a function that satisfies the HJB equation. Here is that equation, with the functions , , and filled in:

Expand the boundary condition:

This function has the form

for some symmetric matrix and some other matrices and that satisfy the following boundary conditions:

Let’s “guess” that this form of is the solution we are looking for, and see if it satisfies the HJB equation. Before proceeding, we need to compute the partial derivatives of :

Here again — just as for LQR — we are applying matrix calculus. Plug these partial derivatives into HJB and we have

To evaluate the minimum, we apply the first-derivative test (more matrix calculus!):

This equation is easily solved:

Plugging this back into (1), we have

where the last step is because

In order for this equation to be true for any , it must be the case that

In summary, we have found that

solves the HJB equation, where , , and are found by integrating the above ODEs forward in time, starting from

The optimal choice of state estimate at time is the choice of that minimizes , that is, the solution to

We can find the solution to this problem by application of the first derivative test, with some matrix calculus:

implying that

Let’s call this solution . Note that we can, equivalently, write

Suppose we take the time derivative of this expression, plugging in what we found earlier for and as well as plugging in yet another version of this same expression, :

For this equation to hold for any , we must have

Behold! This is our expression for an optimal observer, if we define

Finally, suppose we take the limit as , so assume an infinite horizon. It is a fact that tends to a steady-state value, and so does as well. When this happens, , and so the steady-state value of is the solution to the algebraic equation

It is customary to write these last two equations in a slightly different way. In particular, suppose we pre- and post-multiply both sides of this last equation by , and define

Then, we have

and

Summary

An optimal observer — a deterministic, infinite-horizon, continuous-time Kalman Filter — is given by

where

and satisfies

Comparison between LQR and KF (i.e., why you can use “LQR” in Python to compute an optimal observer)

An optimal controller is given by

where

and satisfies

An optimal observer is given by

where

and satisfies

Take the transpose of (5) and — remembering that and are symmetric — we get

Take the transpose of (6) and — remembering that is also symmetric — we get

Compare (3) and (4) with (7) and (8). They are exactly the same if we make the following replacements:

• replace with

• replace with

• replace with

• replace with

• replace with

This is the reason why

L = lqr(A.T, C.T, linalg.inv(Ro), linalg.inv(Qo)).T

produces an optimal observer, just like

K = lqr(A, B, Qc, Rc)

produces an optimal controller. WWWWWOOOOOWWWWW!!!!!!! (See the code used to find the solution to an LQR problem with Python.)