Controlling a Monte Carlo Simulation Kalman Filter

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?

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'

-- 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.

Sorry for the delay, here's my answers:

2. > Your controller would take the state estimate, multiply it by a gain

Well, I'm having problems with both.

2. > Note that with your time-varying Kalman filter in there you're going to

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).

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