Kalman Filter

Consider a servomechanism for the azimuth control of an antenna shown in the figure below. The system has the state vector

"x"=[({{θ}, {\(θ\& . \)}})]

and input and output vectors

({{"u"=[({{V}, {w}})], "and", "y"=[θ]}})

where θ is the angular position of the antenna, V is the input voltage applied to the servo motor, and w is the disturbing torque acting on the motor's shaft. In the following examples we will find the continuous and discrete Kalman estimators. The input "w"(t) and output "v"(t) noise terms will be assumed to be white, mutually uncorrelated noises with zero-mean values.

Because KalmanEstimator provides estimates for output signals, it can be used as a Kalman filter. In this example, we design and try out the Kalman filter, which extracts the useful signal from additive Gaussian noise that masks the angular measurements for the antenna servomechanism.

Load the application.

<< ControlSystems`

Here is a state-space realization of the antenna mechanism.

antenna = 
      StateSpace[{{0, 1}, {0, -5}}, {{0, 0}, {1, 0.1}}, {{1, 
            0}}]

The variance of the process noise is q, and the variance of the measurement noise is r. We now make the measurement noise far more intense and will try to filter it out with the Kalman filter.

q = {{1}}; r = {{100}};

This is the estimator gain matrix.

L = LQEstimatorGains[antenna, q, r, {1}]

Notice that because the process noise was made relatively small and the measurement noise was relatively large, the relatively small gain in the Kalman estimator were achieved. Consequently, the Kalman filter will rely more on re-creating the output signal from the deterministic input using the known system dynamics than on actually processing the noisy sensor output, which is the optimal strategy for noisy measurements.

This is a new object, antenna1, that incorporates another stochastic input through which the measurement noise adds directly to the output. This is done by using the parallel connection with no input connected. The expanded system has three inputs and one output.

antenna1 = 
      ParallelConnect[antenna, TransferFunction[1], {}, {1, 1}]

This finds the Kalman estimator. Note that only the first output of the estimator gives the filtered output of the system.

estimator = KalmanEstimator[%, L, {1}, {1}]

This picks the subsystem we are currently interested in. It contains all the inputs and the first output of the estimator.

filter = Subsystem[estimator, All, {1}]

Then, we connect the expanded system antenna1 and the Kalman filter according to the diagram in the figure below. First we connect the inputs. In the figure, the inputs and outputs are numbered as they appear after this stage.

ParallelConnect[antenna1, filter, {1, 1}, {}]

Finally, we close the feedback loop by connecting the first output of the preceding system with its fourth input (which actually is the first input of the filter). In the composite, the first output corresponds to the output of the system and the second one to the filtered output.

composite = FeedbackConnect[%, {1, 4, Positive}]

To create the normally distributed noise, we will load this standard package.

Needs["Statistics`ContinuousDistributions`"]

Let the length of our simulation sequences be n.

n = 100;

This creates the process noise vector that has the Gaussian distribution with zero mean and standard deviation as required.

w = 
        Table[Random[
            NormalDistribution[
              0, (Sqrt[q])[[1, 1]]]], {n}];

This is the measurement noise vector.

v = 
        Table[Random[
            NormalDistribution[
              0, (Sqrt[r])[[1, 1]]]], {n}];

This is the sinusoidal input signal.

u = Sin[(2/20) π Range[n]] // N;

We intend to supply no external signal to the summing input 4. This prepares a dummy zero signal for this input.

zeros = Table[0, {n}];

This is where the simulation is performed. The result is a list of output vectors.

{y1, y2} = OutputResponse[composite, {u, w, v, zeros}];

We wish to see how the filter suppresses the measurement noise added to the system output signal y1. The "original" signal (before the addition of noise v) is denoted as y10.

y10 = y1 - v;

This plots the signals. We can see that the Kalman filter has quite successfully restored the original signal from the noise.

\(\(MultipleListPlot[y10, y1, y2, PlotRange -> {\(-10\), 10}, 
        PlotJoined -> True, SymbolShape -> None, 
        PlotLabel -> "Signals prior to and after the Kalman Filter", 
        PlotLegend -> {"Original", "Noise Added", "Filtered"}];\)\)