

State Space Control in Verilog/FinSimMath similar to MatLab
This example works on FinSim 10_05_76 and subsequent versions.
This example shows how computations performed in a Tutorial on MatLab
can be computed using FinSimMath.
The code is listed below.
module top;
`include "finsimmath.h"
parameter nr_samples = 200;
parameter size = 3;
parameter p = 1; /* nr_outputs */
parameter m = 1; /* nr_inputs */
parameter n = size;/* nr_states */
VpFCartesian poles[0:size1];
real At[0:2*size1][0:2*size1];
real Bt[0:2*size1][0:m1];
real Ct[0:p1][0:2*size1];
real ut[0:2*m1][0:nr_samples1], Nbar[0:0][0:0];
real K[0:m1][0:size1], L[0:size1][0:p1];
real yt[0:2*p1][0:nr_samples1];
real xt[0:2*size1][0:nr_samples1];
/* declare submatrices and transposed matrices using the "view" construct in
order to share data with the original */
view real y[0:p1][0:nr_samples1] as yt[$I1][$I2];
view real yh[0:p1][0:nr_samples1] as yt[$I1+p][$I2];
view real x[0:size1][0:nr_samples1] as xt[$I1][$I2];
view real xh[0:size1][0:nr_samples1] as xt[$I1+size][$I2];
view real u[0:m1][0:nr_samples1] as ut[$I1][$I2];
view real AK[0:size1][0:size1] as At[$I1][$I2];
view real BK[0:size1][0:size1] as At[$I1][$I2+size];
view real BNbar[0:size1][0:m1] as Bt[$I1][$I2];
view real C[0:p1][0:size1] as Ct[$I1][$I2];
view real Ctran[0:size1][0:p1] as Ct[$I2][$I1];
view real ALC[0:size1][0:size1] as At[$I1+size][$I2+size];
real A[0:size1][0:size1], LC[0:size1][0:size1];
view real Atran[0:size1][0:size1] as A[$I2][$I1];
real B[0:size1][0:m1], D[0:p1][0:m1];
real t0, dt;
real M[0:size][0:size];
view real MA[0:size1][0:size1] as M[$I1][$I2];
view real MB[0:size1][0:m1] as M[$I1][size];
view real MC[0:p1][0:size1] as M[size][$I2];
integer i, j;
/* task rscale uses information from A, B, C, and K in
order to compute Nbar which if multiplied with the input,
compensates the steady state gain */
task rscale;
begin
MA = A;
MB = B;
MC = C;
M[size][size] = D[0][0];
M = M**(1);
Nbar = K*MB;
Nbar[0][0] = Nbar[0][0]+M[size][size];
end
endtask
/* initial block containing computations required by the tutorial. */
initial begin
/* initialization of matrices */
$InitM(At, 0.0);
$InitM(Bt, 0.0);
$InitM(Ct, 0.0);
$InitM(D, 0.0);
/* initialize matrices A, B, and C with data from the linearization of the original differentlial equations */
A = { 0.0, 1.0, 0.0,
980.0, 0.0, 2.8,
0.0, 0.0, 100.0};
B = {0.0, 0.0, 100.0};
C[0][0] = 1.0;
D[0][0] = 0.0;
/* compute poles corresponding to matrix matrix A */
poles = $Eig(A);
$PrintM(poles, "%e");
/* The poles are:
poles[0].Re = 3.130495e+01, poles[0].Im = 0.0
poles[1].Re = 1.000000e+02, poles[1].Im = 0.0
poles[2].Re = 3.130495e+01, poles[2].Im = 0.0
*/
/* initialize the command u, the state variables x[0:2], the distance dt
between two consecutive points in time for which results must be computed,
and the original time t0 */
$InitM(u, 0.0);
x[0][0] = 0.005;
x[1][0] = 0.0;
x[2][0] = 0.0;
dt = 0.01;
t0 = 0;
/* compute y, and x */
y = $LSim(A, B, C, D, u, t0, dt, nr_samples, x);
/* two steps in the tutorial are skipped here, namely
(1) the response of the unstable open loop system and
(2) the placement of another set of poles. The computations are similar to the placement below.*/
/* place poles in a better position, so that the system becomes stable */
poles[0] = 20.0 + 20.0*$i;
poles[1] = 20.0  20.0*$i;
poles[2] = 100;
K = $Place(A, B, poles);
BK = B*K;
AK = ABK;
/* compute y, and x */
y = $LSim(AK, B, C, D, u, t0, dt, nr_samples, x);
/* plot y as shown here .*/
$Flot("Closed_Loop_Response_to_nonzero_initial_state.htm", p, dt,
"Closed_Loop_Response_to_nonzero_initial_state",
"Time (sec)", "Ball position(m)", 0, nr_samples1,
y, "h");
/*
check response to a step input
*/
$InitM(u, 0.001);
x[0][0] = 0.0;
/* compute y, and x */
y = $LSim(AK, B, C, D, u, t0, dt, nr_samples, x);
/* plot y as sown here .*/
$Flot("cLoopRespZandU.html", p, dt,
"Closed_Loop_Response_to_zero_initial_state_and_command",
"Time (sec)", "Ball position(m)", 0, nr_samples1, y, "h");
/* multiply the input with the Nbar coefficient in order to
compensate the steady state gain
*/
rscale;/* user defined system task that produces Nbar */
BNbar = B*Nbar;
/* compute y, and x */
y = $LSim(AK, BNbar, C, D, u, t0, dt, nr_samples, x);
/* plot y as shown here .*/
$Flot("cLoopRespZandUNbar.html", p, dt,
"Closed_Loop_Response_to_zero_initial_state,command,Nbar",
"Time (sec)", "Ball position(m)", 0, nr_samples1, y, "h");
/*
In case the states cannot be directly measured, estimate the
states from the output using an OBSERVER.
The observer should be faster than the original system with feedback.
Therefore it's poles should be at least five times further to the left.
*/
poles[0] = 100.0;
poles[1] = 101.0;
poles[2] = 102.0;
L = $Place(Atran, Ctran, poles);
LC = L*C;
ALC = ALC;
$InitM(u, 0.0);
xt[0][0] = 0.005;
xh[0][0] = 0.005;
/* compute yt and xt */
yt = $LSim(At, Bt, Ct, D, u, t0, dt, nr_samples, xt);
/* plot yt as shown here .*/
$Flot("cLoopRespNZNbarOut.html", 2*p, dt,
"Output_Response_to_nonzero_initial_state,_Nbar,_observer",
"Time (sec)", "Ball position(m)", 0, nr_samples1, yt,
"h", "eh");
/* plot xt as shown here .
In order to zoom in for observing in more detail how the estimated states
track the states of the system one has to select a portion of the large
graph and to zoom out one has to select a portion of the small graph. */
$Flot("State_Response_to_nonzero_initial_state,_Nbar,_observer.htm", 2*n, dt,
"State_Response_to_nonzero_initial_state,_Nbar,_observer",
"Time (sec)", "Ball position(m)", 0, nr_samples1,
xt,"h","v", "i", "eh", "ev", "ei");
end
endmodule
