Why State-space output giving unstable behavior in designing Kalman filter
6 次查看(过去 30 天)
显示 更早的评论
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')
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1296345/image.png)
6 个评论
回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!