Overview
Comment:Include LQ PPP option
General tidy
Downloads: Tarball | ZIP archive
Timelines: family | ancestors | descendants | both | origin/master | trunk
Files: files | file ages | folders
SHA3-256: 26fc6f2cc443b0300df994aa8f79729d07bea127de30ef271ce9bb6f7c324864
User & Date: gawthrop@users.sourceforge.net on 2003-06-06 07:18:27.000
Other Links: branch diff | manifest | tags
Context
2003-06-09
10:49:23
Include error in out args check-in: 09dff22e59 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
2003-06-06
07:18:27
Include LQ PPP option
General tidy check-in: 26fc6f2cc4 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
07:17:28
Removed debug check-in: 6d7ce96d72 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
Changes
Side-by-Side Diff Ignore Whitespace Patch
Modified mttroot/mtt/lib/control/PPP/ppp_lin_run.m from [566d14aa8d] to [33262152e0].
︙
17
18
19
20
21
22
23





24
25
26
27
28
29
30

17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35








+
+
+
+
+









  ##Defaults
  if nargin<1			# Default name to dir name
    names = split(pwd,"/");
    [n_name,m_name] = size(names);
    Name = deblank(names(n_name,:));
  endif

  ## System
  sys = mtt2sys(Name);		# Create system
  [A,B,C,D] = sys2ss(sys);	# SS form
  [n_x, n_u, n_y] = abcddim(A,B,C,D); # Dimensions
  
  if nargin<2
    Simulate = 1;
  endif
  
  if nargin<3
    ControlType = 2;
︙
38
39
40
41
42
43
44

45
46
47



48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65







66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84

43
44
45
46
47
48
49
50



51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84






85
86
87
88
89
90
91








+
-
-
-
+
+
+


















+
+
+
+
+
+
+






-
-
-
-
-
-








    p_c.N = 10;
  endif
  
  if nargin<6
    p_o.sigma = 0.001;
  endif


  if !struct_contains(p_c,"N")
    p_c.N = 10;			# Number of small samples per large sample
  endif
#   if !struct_contains(p_c,"N")
#     p_c.N = 10;			# Number of small samples per large sample
#   endif
  
  if !struct_contains(p_c,"delta_ol")
    p_c.delta_ol = 1.0;	# OL sample interval
  endif
  
  if !struct_contains(p_c,"T")
    p_c.T = 5.0;			# Last time point.
  endif
  
  if !struct_contains(p_c,"A_w")
    p_c.A_w = 0;
  endif
  
  if !struct_contains(p_c,"A_u")
    p_c.N_u = 3;
    a_u = 2.0;
    p_c.A_u = ppp_aug(p_c.A_w,laguerre_matrix(p_c.N_u-1,a_u));
  endif
  
  if !struct_contains(p_c,"Method")
    p_c.Method = "lq"; 
    p_c.Q = eye(n_y);
    p_c.R = (0.1^2)*eye(n_u);
    p_c.N_u = n_x;
  endif
  
  [p_c.N_u,M_u] = size(p_c.A_u);
  if (p_c.N_u<>M_u)
    error("A_u must be square");
  endif
  
  ## System
  sys = mtt2sys(Name); # Create system
  [A,B,C,D] = sys2ss(sys);	# SS form
  [n_x, n_u, n_y] = abcddim(A,B,C,D)
  ol_poles = eig(A)

  ## Check w.
  [n_w,m_w] = size(w);
  if ( (n_w<>n_y) || (m_w<>1) )
    error(sprintf("ppp_lin_run: w must a column vector with %i elements",n_y));
  endif
  
  ## Initialise
︙
97
98
99
100
101
102
103

104
105
106











107
108

109
110
111
112
113
114
115
116
117

118
119
120
121
122
123
124
125
126
127
128



129
130
131
132
133
134
135

104
105
106
107
108
109
110
111



112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128




129

130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151








+
-
-
-
+
+
+
+
+
+
+
+
+
+
+


+



-
-
-
-

-
+











+
+
+








    K_w = zeros(p_c.N_u,n_y);
    K_w(1,1) = 1;
    K_w(2,1) = -1;
    K_x = zeros(p_c.N_u,n_x);
    U = K_w*w;			# Initial control U
  else				# PPP control
    I = ceil(p_c.T/p_c.delta_ol); # Number of large samples
    if strcmp(p_c.Method, "original")
    tau = [10:0.1:11]*(2/a_u);	# Time horizons
    [k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,tau); # Design
    U = K_w*w			# Initial control U
      tau = [10:0.1:11]*(2/a_u);	# Time horizons
      [k_x,k_w,K_x,K_w] = ppp_lin(A,B,C,D,p_c.A_u,p_c.A_w,tau); # Design
    elseif strcmp(p_c.Method, "lq")
      tau = [0:0.001:1.0]*5; # Time horizons
      [k_x,k_w,K_x,K_w,Us0,J_uu,J_ux,J_uw,J_xx,J_xw,J_ww,y_u,p_c.A_u] \
	  = ppp_lin_quad (A,B,C,D,tau,p_c.Q,p_c.R);
    else
      error(sprintf("Method %s not recognised", p_c.Method));
    endif
    
    U = K_w*w;			# Initial control U

    ## Checks
    [ol_zeros, ol_poles] = sys2zp(sys)
    cl_poles = eig(A - B*k_x)
  endif

  ## Sample times
  dt = 0.1;
  delta = p_c.N*dt;

  ## Observer design
  Ad = expm(A*delta);		# Discrete-time transition matrix
  Ad = expm(A*p_c.delta_ol);		# Discrete-time transition matrix
  if (ControlType==2)
    G = eye(n_x);			# State noise gain 
    sigma_x = eye(n_x);		# State noise variance
    Sigma = p_o.sigma*eye(n_y);	# Measurement noise variance
    
    L = dlqe(Ad,G,C,sigma_x,Sigma)
  else
    L = zeros(n_x,n_y);
  endif
  
  obs_poles = eig(Ad-L*C)

  ## Short sample interval
  dt = p_c.delta_ol/p_c.N;

  ## Control loop
  y = [];
  u = [];
  t = [];
  y_e = [];
  t_e = [];
︙
145
146
147
148
149
150
151
152

153
154
155
156
157
158
159

161
162
163
164
165
166
167

168
169
170
171
172
173
174
175








-
+








      data = from_rt(p_c.N);	# Receive data
      [yi,ui] = convert_data(data);
      y_now = yi(:,p_c.N);	# Current output
    endif
    

    ## Zero-gain (OL) observer with state resetting
    [x_est y_est] = ppp_int_obs (x_est,y_now,U,A,B,C,D,p_c.A_u,delta,L);
    [x_est y_est] = ppp_int_obs (x_est,y_now,U,A,B,C,D,p_c.A_u,p_c.delta_ol,L);
    
				#       ## Reset states
				#       x_est(2) = y_now(1);	# Position
				#       x_est(4) = y_now(2)/g_s;	# Angle 
    
    ##Control
    U = K_w*w- K_x*x_est;
︙
MTT: Model Transformation Tools
[ GitHub | SourceHut | Sourceforge | Fossil RSS ]