Digital Control Octave Scripts

System Identification Simulation

% Digital Control System Identification Example
pkg load control
clear all;
close all;
% Set up the system to fit to.
A=diag([-1,-2]) % Make sure the system is stable.
B=[1,1]'
C=[1,2]
%A=diag(-1)
%B=1
%C=1
sys = ss(A,B,C); % sys is the system to fit.
% Move to the sampled domain.
T=0.01; % T is the sampling interval.
N=10000; % N is the number of training points.
t=0:T:N*T;
sys_discrete = c2d(sys,T);
sys_discrete_tf = tf(sys_discrete)
if(!isstable(sys)) 
  error('System is not stable!\n') % Check for stability.
 end
if(!isctrb(sys)) % Check to make sure the system is contrololable,
  error('System is not controllable!\n')
end
if(!isobsv(sys)) % and observable.  If it is not you need to fix it!
  error('System is not observable!\n')
end
% Now set up the system identification.
u=randn(size(t));
[y,t]=lsim(sys,u,t);
% The order of the 0fitted system is n.
n=2
Y=[[y(n+1:N+1)]]; % Note: Octave's lowest index is 1, not 0.
for k=n:N
  PSI(k-n+1,:)=[y(k:-1:k-n+1)', u(k:-1:k-n+1)];
end
PSI;
THETA=(PSI'*PSI)^(-1)*PSI'*Y
sys_fit=tf(THETA(n+1:2*n),[1,-THETA(1:n)'],T) % tf(num,den,T)
[y_fit,t]=lsim(sys_fit,u,t);
err=(y-y_fit);
plot(t,err)
figure;
plot(t,y,t,y_fit)
sum_err = (y-y_fit)'*(y-y_fit)

Kalman Filter RC Circuit Example

This is for a series RC circuit. It shows how to do it yourself using equations we derived in class and using dlqe().

% This is a demo of the Kalman filter on an RC circuit.
% Rob Frohne, for ENGR 454, Digital Control Systems.
% We will use v_dot = A *v + B*u, where A=-1/RC.  The input is set to 
% u(t) = (1 + sign(t))/2.
clear all;
pkg load control;
R = 3300;
C = 1000e-6;
A = -1/(R*C);
B = 1000;%/(R*C);
C_cont = 1;
cont_sys = ss(A,B,C_cont);
T = 0.1;  % sampling interval
N=200;
disc_sys = c2d(cont_sys,T);
Phi = disc_sys.a;
Gamma = disc_sys.b;
%C = disc_sys.c;
t=(0:T:N*T)';
u = 1*(sign(t));
[y,t,x] = lsim(disc_sys,u) ; % The original discretized system without any noise.
w = .01*randn(size(t));  % random state noise
v = sqrt(.01)*randn(size(t));  % random measurement noise
% Note:  to add w to x and retain u, u = u + 1/Gamma*w
[yn,t,xn] = lsim(disc_sys,u+1/Gamma*w);  %noisy due to state errors.
yn = yn + v;  % noisy due to measurement errors.
Q = cov(w,w')
R = cov(v,v')

% First: show how to use the built in kalman function to get steady state results.

%  *Block Diagram*
%                                           u  +-------+         ^
%                +---------------------------->|       |-------> y
%                |    +-------+     +       y  |  est  |         ^
%          u ----+--->|       |----->(+)------>|       |-------> x
%                     |  sys  |       ^ +      +-------+
%          w -------->|       |       |
%                     +-------+       | v
%
%          Q = cov (w, w')     R = cov (v, v')     S = cov (w, v') 

[m,p,z,e] = dlqe(Phi,[],disc_sys.c,Q,R)  

% Now simulate the Kalman filter step by step.
x_hat(1) = 0; % initial x
x_tilde(1) = x_hat(1); % for graphing purposes.
p_hat(1) = 0; % initial p_hat
p_tilde(1) = p_hat(1);
for j = 2:length(t)
  p_tilde(j) = Phi*p_hat(j-1)*Phi' + Q;
  x_tilde(j) = Phi*x_hat(j-1) + Gamma*u(j-1);
  Kj(j) =p_tilde(j)*disc_sys.c'/(disc_sys.c*p_tilde(j)*disc_sys.c'+R);
% p_hat(j)= (1-Kj(j)*disc_sys.c)*p_tilde(j); % Numerically unstable
  p_hat(j)=(1-Kj(j))*p_tilde(j)*(1-Kj(j))' + Kj(j)*R*Kj(j)'; % Numerically stable
  x_hat(j) = x_tilde(j) + Kj(j)*(yn(j) - disc_sys.c*x_tilde(j));
endfor
disp("Our Kj ended at: "),disp(Kj(end)) 
disp("The dlqe estimate of K is: "),disp(m)
figure(1)
plot(p_hat)
title('Estimate of the Variance')
figure(2)
plot(t,x,t,x_tilde,t,yn)
title('RC Circuit Response')
legend('No Noise','Kalman Estimate','Noisy Measurement')
figure(3)
plot(Kj)
title('Kj');