I am a dsp guy, I only did a minimum of control theory back in university. While trying to grok state space analysis and (discrete time) regular Kalman filters, I am hitting a few questions that google/wikipedia/my book on control theory is unable to enlighten me on. If you excuse my somewhat verbose post as an attempt to explain what Kalman is doing using my own words, trying to get feedback on what parts I am misunderstanding (not even getting into why it is doing this).
Say that a system is modeled as (state-space):
This system can be described by the difference equations:
x(k) = A*x(k-1) + B*u(k-1) + q(k-1)
y(k) = H*x(k) + D*u(k) + r(k)
Where:
- capital letters represents matrix
- sub capital represents vectors,
- q ~gaussian(0,Q). r ~gaussian(0,R),
- u(k) is external (known) stimuli,
- x(k) is internal (unknown) state,
- y(k) is (known) system output,
- k is (scalar) discrete time,
similar to references at the bottom. Using pseudo-Matlab notation throughout. All stochastic variables are assumed independent Gaussians.
Trying to super impose the Kalman thing on top in a somewhat hand-wavy way, we are doing a two-phase estimation of the hidden state for each time (k) increment:
So basically state and output is modelled as gaussian distributions that have slowly changing means (to be estimated), with additive zero-mean gaussian noise on top (to be cancelled)?
"The most difficult task is figure out how to formulate an estimation problem in state space form." {1} "In most applications, the internal state is much larger (more degrees of freedom) than the few "observable" parameters which are measured. However, by combining a series of measurements, the Kalman filter can estimate the entire internal state." {2}
What is it that the A matrix ("state transition" or "dynamic" model) along with the expansion into some number of hidden state dimensions really does? I imagine that for variables that cannot be observed, it is about enforcing degree of smoothness (continuity) in x, its derivative and so on? For a physical system one might assume that a space rocket does not jump immediately from position#1 to position#2 (infinite velocity) or from velocity#1 to velocity#2 (infinite acceleration = infinite force). Is that it? For something like the stock market it seems more abstract to apply similar kind of rules, but perhaps that is only me not being an economist.
Or is it more about being able to accept different sensory inputs that might measure (either) x, or its n-th derivative with some accuracy and (typically) have high-frequency noise as you derivate the "natural" sensory output or drift as you integrate it? If sensor fusion is "the thing" with Kalman filters, there would seem to be other (simpler) methods that can accomplish somewhat similar things (my control theory book mentions complementary filters, where a simple filter bank lets in high-frequency information from one sensor, low-frequency from another).
The "prediction" phase consists of making a prediction about the next state, based only on "old" information: the previous state along with the dynamic system description (an object traveling at 100km/h in some direction at time k-1 have an expected location at time k unless external forces, u() changed its course), or noise caused some error in actual state or measurement thereof:
x(k) = A * x(k-1) + B * u(k-1);
Also, the state covariance is predicted recursively, baking in process noise:
P(k) = A * P(k-1) * A' + Q;
The user is expected to present initial covariance estimates for state P and state noise Q. P will be updated throughout, while Q is static. So how should one think about the difference between an assumed stationary additive noise source, and the observed (sample) state covariance?
In the "update" phase, current measurements are integrated in the model and compared to the prediction result. Measurement and model covariance decide how much relative weight to put in the new measurement.
Project state to output, innovation/measurement residual (prediction error?)
e = y(k)-H*x(k);
Innovation/measurement prediction covariance
IS = H*P*H' + R;
%% Kalman gain
Kg = P*H'/IS;
%% update state estimate, weighting in current measurement residual
x(k) = x(k) + Kg * e;
%% update state covariance
P = P - Kg*IS*Kg';
"The Kalman filter produces an estimate of the state of the system as an average of the system's predicted state and of the new measurement using a weighted average. The purpose of the weights is that values with better (i.e., smaller) estimated uncertainty are "trusted" more... With a high gain, the filter places more weight on the most recent measurements, and thus follows them more responsively. With a low gain, the filter follows the model predictions more closely. At the extremes, a high gain close to one will result in a more jumpy estimated trajectory, while a low gain close to zero will smooth out noise but decrease the responsiveness." {2}. Sort of like exponential smoothing {4}, only the cutoff frequency being adaptive and based on system assumptions rather than ad-hoc tuning?
But being able to choose the weight to put on each input sample based on the distance from a pre-calculated mean estimate, even for a first-order smoother, means that there is some "outlier suppression". I.e. a nonlinear filtering operation, similar (in kind) to a median filter?
Edit: Aided by the excellent response from @peter-k I have thought about the problem some more.
I still feel that I don't have a good grip on what this 'A' matrix really does. It should relate the state vector (2-element in this case) from time k-1 to time k. But where is it explicitly encoded that x1 is 'mean' while x2 is derivative? And why should the off-diagonal "leakage" terms have any particular value?
Experiment setup:
%adopted from https://github.com/EEA-sensors/ekfukf
%% Create step signal, then corrupt it with noise
N2 = 150;
X(1:N2) = -1;
X((N2+1):2*N2) = 1;
sd = 0.1;%measurement noise standard deviation
rng('default')
Y = X + sd*randn(size(X));
M = [0; 0];%Mean state estimate
P = diag([0.1 2]);%NxN state initial covariance. Jump start by large diagonal
R = sd^2;%Measurement noise covariance (scalar).
H = [1 0];%observe only x
A = eye(2)+[0 0.1; 0 0];%??
Q = diag([1e-4 1e-3]);%tune parameter
Execution loop:
for k=1:size(Y,2)
%% Predict
M = A * M;
P = A * P * A' + Q;
%% Update
e = (Y(k)-H*M);
IS = (R + H*P*H');
Kg(:,k) = P*H'/IS;
M = M + Kg(:,k) * e;
P = P - Kg(:,k)*IS*Kg(:,k)';
%% stash variables for plotting
MM(:,k) = M;
PP(:,:,k) = P;
end
discussion:
I had somehow thought that the Kalman gain would fluctuate for outlier noise (or for large steps in the noise-less signal), thus being somewhat nonlinear/time-variant. I see here that it converges to a constant. Some smoothing is observed, and some smearing/overshoot is observed. Tuning P, Q and A seems to be key here, and it seems to be non-trivial, see e.g. {6}
references:
{1}https://users.aalto.fi/~ssarkka/course_k2011/pdf/handout1.pdf
{2}https://en.wikipedia.org/wiki/State-space_representation
{3}https://en.wikipedia.org/wiki/Kalman_filter
{4}https://en.wikipedia.org/wiki/Exponential_smoothing
{5}https://stanford.edu/class/ee363/lectures/kf.pdf
{6}https://minds.wisconsin.edu/bitstream/handle/1793/10890/file_1.pdf?sequence=1&isAllowed=y