Controlling a Monte Carlo Simulation Kalman Filter

jmathgeek@gmail.com wrote:

In what way are you experiencing difficulty?  Certainly to do what you
say with what you say you have (and it appears your code is what you say
it is) you'll need to rearrange the code to execute the Kalman filter at
the same time as the process is running, because the Kalman filter will
be part of the process.

I have some general comments about your code.  It looks OK, but it's a
'system's engineer's' code, not a software engineer's.

In general, you need more comments.  Some specific things that I noticed
are that you are using short variable names that are canonical in the
literature (F, G, W, K, P) without commenting.  Unfortunately, not
everyone remembers what these canonical variable names are, and some
authors choose different variable names.

Commenting the variable names with their 'expanded' names (i.e. process
state evolution matrix, Kalman state evolution matrix, covariance
matrix, etc.) will help.  I would either comment these in one block
toward the head of the function, or as you define the variable.

A comment block on your general approach is a good idea; you should
include bibliographic references to any books that you used heavily,
particularly if you propped the book up next to your keyboard and wrote
the code straight from equations 6.7 through 6.13.

I would split what you have into several functions.  I would have at
least a combined process/Kalman simulation and a separate 'test bench'
to run them; I would certainly seriously consider a general-purpose
Kalman filter that I could use other places just by setting up the gain
matrices differently.  I'd probably also have a separate function call
for the physical process.  Deciding whether to tie these together in the
test bench or a third function call is a tactical decision depending on
what else you'll do with it.

Splitting up your code like that may seem to make things harder to
understand, but in the long run (even over the span of a week or two)
you should find that it helps, particularly if you document it with a
top-level block diagram showing how each function neatly implements one
block.

My one specific comment is that in the loop labeled "generate the
process" it appears that you are generating just one random variable and
applying it in three places -- normally the model that the Kalman is
working off of would have one R.V. per state, each working independently
through a covariance matrix.  Are you really modeling what you intend to
model?


--

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" gives you just what it says.
See details at http://www.wescottdesign.com/actfes/actfes.html

I've now made comments on variables, and also references to
"Introduction to Random Signals and applied Kalman Filtering",
Brown and Hwang, 1997. That's where the formulas (and variable names)
come from.
I've not split it into functions, but I find it easier to keep it
like it is, for right now, for this newsgroup purpose
(now you can simply copy and paste it into Matlab, and run it).

Regarding u in the Monte Carlo simulation:

u = [randn;
    randn;
    randn;];

gives three different random numbers, example:

u =[ 0.2877
   -1.1465
    1.1909]


I now also execute the Kalman filter at the same time as the process
is running.

The program code follows right under here.

Now it should be possible to implement a control of the system, right?
How would you do that?



function Simulation_2_order_Gauss_Markov_pva

%**********************************************
% Simulation, moving 1-dimensional position.
% Kalman filter / process:
% 2. order Gauss-Markov, pva-model (3-state).
% Process on v, observation on p.
% Monte Carlo simulation.
%**********************************************


sigma_sq = 15;

Rk = 2; %Measurement error

dt = 1;

sigma = sqrt(sigma_sq); %sigma in the 2. order Gauss-Markov process

beta = 0.2;  %beta in the 2. order Gauss-Markov process
% (1/5 for correlation time 5*dt.)


% x' = Fx + Gu

F = [0 1 0
    0 0 1
    0 -(beta^2) -(2*beta)];

G = [0 0 0
    0 0 0
    0 0 (2 * sigma * (beta^(3/2)))];

W = [0 0 0
    0 0 0
    0 0 1];







%Calculating Phi and Qk by VanLoan's method:
% (ref. Introduction to Random Signals and applied
% Kalman Filtering, Brown
% and Hwang, 1997. Example 5.5 page 205.)

no = size(F,2);

null = zeros(no,no);
I = eye(no,no);

A = [-F*dt G*W*G'*dt;
    null F'*dt;];


B = expm(A);


Pu = [B(4,4) B(4,5) B(4,6);
    B(5,4) B(5,5) B(5,6);
    B(6,4) B(6,5) B(6,6);];

Phi = Pu'; %State transition matrix Phi


Ku = [B(1,4) B(1,5) B(1,6);
    B(2,4) B(2,5) B(2,6);
    B(3,4) B(3,5) B(3,6);];

Qk = Phi*Ku; %Process noise covariance matrix Qk


Hk = [1 0 0;]; %Measurement Design matrix Hk -
% gives connection between measurement and state vector






%Initializing the variables:

P_ = [1 0 0;
    0 1 0;
    0 0 1;];

x_ = [0;
    0;
    0;];


n = 50;

b = 0;

x = zeros(3,n+b);
x_process = zeros(3,n+b);

std_x_p = zeros(1,n+b);
K = zeros(3,n+b);

z = zeros(1,n+b);






%Running the Simulation and Kalman filter:

for i = 2:(n+b)

    %Generate the process:
    % (ref. Introduction to Random Signals and applied
    % Kalman Filtering, Brown and Hwang, 1997. Section 5.4.)
    u = [randn;         % u consists of three different random
variables
        randn;
        randn;];

    C = chol(Qk);
    C = C';
    wk = C*u;
    x_process(:,i) = Phi * x_process(:,i-1) + wk; %Simulated Process
state vector

x_process



    %Kalman Filter:
    % (ref. Introduction to Random Signals and applied Kalman
Filtering, Brown
    % and Hwang, 1997. Fig. 5.8 Page 219)
    K(:,i) = P_ * Hk' * (Hk * P_ * Hk' + Rk)^-1;    %Kalman Gain K

    z(i) = x_process(1,i) + (sqrt(Rk) * randn); %Measurement z

    x(:,i) = (x_ + K(:,i)*(z(i) - Hk*x_));  %Process state vector x

    P = (I - K(:,i) * Hk) * P_; %Error Covariance matrix P

    x_ = Phi * x(:,i);  %Projected process state vector x_
    P_ = Phi * P * Phi' + Qk; %Projected Error Covariance matrix P_


    std_x_p(i) = sqrt(P(1,1)); %std position
end





%Plotting results:
process_x_p = x_process(1,(b+1):(b+n));
x_p = x(1,(b+1):(b+n));
z = z(1,(b+1):(b+n));
std_x_p = std_x_p(1,(b+1):(b+n));
K = K(1,(b+1):(b+n));


i = 1:n;
plot(i,process_x_p,'-',i,x_p,i,z,'^');   %,i,std_x_p,'-',i,K,'-'
legend('true process', 'filter estimate', 'observations');
%, 'filter error (sqrt(P(1,1)))', 'Kalman gain'

jmathgeek@gmail.com wrote:

-- snip --

Your controller would take the state estimate, multiply it by a gain
vector, and feed it back to the plant input.  The algorithm goes

- get latest feedback
- iterate Kalman filter
- multiply state estimate by gains
- iterate plant simulation
- repeat

Is that what you were having difficulty with, or were you concerned with
constructing the gains?

Note that with your time-varying Kalman filter in there you're going to
have all sorts of interesting issues with analyzing the system for
stability if the plant doesn't match the model upon which the Kalman
filter is based -- are you prepared to do that analysis?

Also (and certainly least important!), since you appear to be on Google,
search around to see how to retain context with newsgroup posts from
Google.  Either through ignorance, laziness, or arrogance Google doesn't
have a really good USENET interface; consequently you have to work to
make your posts adhere to good USENET usage.

--

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Do you need to implement control loops in software?
"Applied Control Theory for Embedded Systems" gives you just what it says.
See details at http://www.wescottdesign.com/actfes/actfes.html

Sorry for the delay, here's my answers:

1.

Well, I'm having problems with both.


2.

Since my Phi (State transition matrix) and Qk (Process noise
covariance matrix) matrixes are constant, I don't think the Kalman
filter is time varying?

(The Kalman filter goes into a steady state condition (constant Kalman
gain) after about 10-15 iterations).

On Aug 4, 4:25 am, jmathg...@gmail.com wrote:

Given what you have said this seems about right.  10-15 does seem a
little quick but it all depends on the values in your arrays.  If you
know the Qk and r you can calculate the Kalman gains directly for 2x2
or 3x3 arrays instead of going through all those iterations with the
Riccati equation.

Peter Nachtwey