Why State-space output giving unstable behavior in designing Kalman filter

Hello MATLAB Community,

My final objective is to find a Kalman filter gain matrix. I have expanded state space matrix, A, C from the Plant model. The entire code is shown below:

When I am solving for states and ouput of the system, it gives unstable behaviour (a very high values of y). Due to this reason, Kalman filter is not giving desired results. It would be really nice, if some one help me in this regards. Thanks in advance!

clear;
clc;
% Find the gain of the Kalman Filter (K)

Ax1 = [-967.2 -4226 -295.6 -2507 -169.2 -1690];
Ax2 = [16384 8192 4096 4096 2048]';
Ax = [Ax1; diag(Ax2), zeros(5,1)];

Bx = [64 0 0 0 0 0]';
Cx = [0 24.19 1.446 25.91 1.61 25.36];
Dx = 0;

A = [Ax, Bx; zeros(1,6), zeros(1,1)];
C = [Cx, 0];

Q = 1e9;
R = 32.58;
NN =  [zeros(1,6), 1]';

StateDim = size(A,1) ;                     % Number of states ( size(A,1) )
ObsDim = size(C,1);                        % Number of observations ( size(C,1) )

ts = 0.1e-3;
N  = 5000;                         % Number of datapoints
X  = zeros(StateDim,N);            % State data buffer
y  = zeros(ObsDim,N);              % Observation data buffer
t = ts*(0:N-1);


%% Generate Process Noise (Noise of the state equations)
Var_PNoise = .1;                   % Process Noise variance
Mu_PNoise = 0;                     % Process Noise mean
Std_PNoise = sqrt(Var_PNoise)';    % Standard deviation of the process noise
w = Std_PNoise * randn(StateDim,N) + Mu_PNoise*ones(StateDim,N);    % Gaussian Process Noise
% Q = cov(PNoise');                  % Process Noise Covariance Matrix

%% Generate Observation Noise (Noise of the output equation0
Var_ONoise = 2;                    % Observation Noise variance
Mu_ONoise = 0;                     % Observation Noise mean
Std_ONoise = sqrt(Var_ONoise)';    % Standard deviation of the observation noise
v = Std_ONoise * randn(ObsDim,N) + Mu_ONoise*ones(ObsDim,N);    % Gaussian Observation Noise
% R = cov(ONoise');                  % Observation Noise Covariance Matrix

% Initial values for the model
X = [1, zeros(1, 6)]';          % Initial state
y = C * X(:,1) + 0;             % Initial observation

% Simulate states and observations
for i = 1 : N
    X = A*X + NN*w(1,i); % States
    y(:,i) = C*X   + v(1,i); % Real Observations,
end

plot(t, y)
%% Kalman filtering...
xh(:,1) = 0.01*randn(StateDim,1);    % Initial state
Px = eye(StateDim);                  % Initial state covariance matrix

for i = 1 : size(y,2)

    %---------- Time Update ----------
    % A priori estimate of the current state ( x(t|t-1) = A*x(t-1|t-1) )
    xh_1(:,i) = A * xh(:,i);
   
    % A priori estimate of the state covariance matrix ( Pd(t|t-1) = A*P(t-1|t-1)*A' + N*Q*N' - P(t-1|t-1)*C'*R^-1*C*P(t-1|t-1)
    Pdx = A*Px + Px*A' + NN*Q*NN' - Px*C'*(R^-1)*C*Px;
   
    %---------- Measurement Update ----------
    % Kalman filter coefficient ( K(t) = P(t-1|t-1)*C'*R^-1)
    K = Pdx * C' * (R^-1);
   
    % Estimated observation ( y(t|t-1) = C*x(t|t-1) + R )
    yh_1(:,i) = C * xh_1(:,i) + R;
   
    % Measurement residual error or innovation error ( y(t) - y(t|t-1) )
    inov(:,i) = y(:,i) - yh_1(:,i);
   
    % A posteriori (updated) estimate of the current state ( x(t|t) = x(t|t-1) + K(t)*(y(t)-y(t|t-1)) )
    xh(:,i+1) = xh_1(:,i) + K* inov(:,i);
   
    % A posteriori (updated) state covariance matrix ( P(t|t) = (I - K(t)*C) * P(t|t-1) )
    Px = Pdx - K*C*Pdx;

end
   
%% Plot the estimation results   
figure, plot(y,'b')
hold on, plot(yh_1,'r')
grid on
legend('Real observation','Estimated observation')   

[Askic V]

Most probably it is because your system simulation is not correcty defined. Please try to simulate the system and look how its output (y variable) is changing without any noise added. It seems to me that you don't have stable system in the first place, so Kalman filter (which is optimal linear estimator) is not stable because your initial output is not stable.

State space mode of the system is: X(n+1) = Ax(n)+Bu, y(n) = C*X(n), where u is external input. Try to get stable response of the system first and then add noise and try to design Kalman filter to make predictions of the output value.

[Govind Narayan Sahu]

Thanks!

Even without adding noise, it is still giving same unstable behaviour. I checked the root locus plot of the base system (sys = ss(Ax, Bx, Cx, Dx)), it is stable system, all poles and zeros are in the left side. I also looked the root locus plot of the expanded system (sys1 = ss(A, ([zeros(1,6), 1]'), C, 0)), poles are in left side but its locus is crossing imaginary axis at ceratin gain

I am wondering, how this author has implemented this https://open.library.ubc.ca/media/stream/pdf/24/1.0377725/3 (see, pages 25-38).

[Askic V]

Do you know there is a difference between discrete and continuous models?

In this thesis, model is developed in the continuous time. State space matrices A,B,C and D are not the same in the discrete domain and they are dependent on the value of sampling time. Your root locus plot is working with continuous model.

For example, if model is converted to the discrete domain (needed for writing scripts and estimate discrete values of output y), then without any noise, with the following script you'll get the following output:

%%

clear;
clc;
% Find the gain of the Kalman Filter (K)
Ax1 = [-967.2 -4226 -295.6 -2507 -169.2 -1690];
Ax2 = [16384 8192 4096 4096 2048]';
Ax = [Ax1; diag(Ax2), zeros(5,1)];
Bx = [64 0 0 0 0 0]';
Cx = [0 24.19 1.446 25.91 1.61 25.36];
Dx = 0;

u = 1; % external input is a constant
% convert to discrete domain
Ts = 0.1e-3; % sampling time
[Adx,Bdx,Cdx,Ddx] = c2dm(Ax,Bx,Cx,Dx,Ts);
%%%%%%%%%%%%%%%%

NN =  zeros(1,6)';
StateDim = size(Adx,1);            % Number of states ( size(A,1) )
ObsDim = size(Cdx,1);         
N  = 5000;                         % Number of datapoints
X  = zeros(StateDim,N);            % State data buffer
y  = zeros(ObsDim,N);              % Observation data buffer
t = Ts*(0:N-1);

X = zeros(1, 6)';          % Initial state
y = Cdx * X(:,1);              % Initial observation
% Simulate states and observations
for i = 1 : N
    X = Adx*X+Bdx*u ; % States
    y(:,i) = Cdx*X;   %+ v(1,i); % Real Observations,
end
plot(t, y)
xlabel('t');
ylabel('Output y');

I'm pretty sure you'll like this plot much better.

[Govind Narayan Sahu]

Thanks Askic,

I got it, you solved my problem. Many Thanks!

[Askic V]

@Govind Narayan Sahu if that is the case, please accept this reply as an answer.

[Govind Narayan Sahu]

ok