I've made a simple matlab program:
a Monte Carlo simulation for Kalman filtering a moving, 1-dimensional
position.

The process is 2. order Gauss-Markov, and I use a pva-model;
the process is on v, and I have the observations on position.

Monte Carlo is used to simulate the process, then I filter it with the Kalman filter afterwards.

Attached is the matlab program.

I now want to expand this simulation with a control loop that controls the process / the moving position, so that position is as close to zero as possible. (We will then need to simulate just one step at a time.)

Could anyone help?

Attached: Simulation_2_order_Gauss_Markov_pva.m

function Simulation_2_order_Gauss_Markov_pva

%

sigma_sq = 15;

Rk = 2;

dt = 1;

sigma = sqrt(sigma_sq);

beta = 0.2; %(1/5 for correlation time 5*dt.)

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

G = [0 0 0 0 0 0 0 0 (2

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

%Calculating Phi and Qk by VanLoan's method:

n = size(F,2);

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

A = [-F

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';

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;

Hk = [1 0 0;];

%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_fasit = zeros(3,n+b);

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

z = zeros(1,n+b);

%Generate the process:

for i = 2:(n+b)

% randn('state', sum(100*clock));

u = [randn; randn; randn;];

C = chol(Qk); C = C'; wk = C

end

%Running the Kalman filter:

for i = 2:(n+b)

K(:,i) = P_

z(i) = x_fasit(1,i) + (sqrt(Rk) * randn);

x(:,i) = (x_ + K(:,i)

x_ = Phi

std_x_p(i) = sqrt(P(1,1)); end

%Plotting results: fasit_x_p = x_fasit(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,fasit_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'

The process is 2. order Gauss-Markov, and I use a pva-model;

Monte Carlo is used to simulate the process, then I filter it with the Kalman filter afterwards.

Attached is the matlab program.

I now want to expand this simulation with a control loop that controls the process / the moving position, so that position is as close to zero as possible. (We will then need to simulate just one step at a time.)

Could anyone help?

Attached: Simulation_2_order_Gauss_Markov_pva.m

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;

dt = 1;

sigma = sqrt(sigma_sq);

beta = 0.2; %(1/5 for correlation time 5*dt.)

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:

n = size(F,2);

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

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';

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;

Hk = [1 0 0;];

%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_fasit = zeros(3,n+b);

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

z = zeros(1,n+b);

%Generate the process:

for i = 2:(n+b)

% randn('state', sum(100*clock));

u = [randn; randn; randn;];

C = chol(Qk); C = C'; wk = C

***u; x_fasit(:,i) = Phi ***x_fasit(:,i-1) + wk;end

%Running the Kalman filter:

for i = 2:(n+b)

K(:,i) = P_

*** Hk' ***(Hk*** P_ ***Hk' + Rk)^-1;z(i) = x_fasit(1,i) + (sqrt(Rk) * randn);

x(:,i) = (x_ + K(:,i)

***(z(i) - Hk***x_)); P = (I - K(:,i)*** Hk) ***P_;x_ = Phi

*** x(:,i); P_ = Phi ***P * Phi' + Qk;std_x_p(i) = sqrt(P(1,1)); end

%Plotting results: fasit_x_p = x_fasit(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,fasit_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'