Overview
Comment:Include LQ PPP option
General tidy
Downloads: Tarball | ZIP archive | SQL archive
Timelines: family | ancestors | descendants | both | origin/master | trunk
Files: files | file ages | folders
SHA3-256: 17b804049396885b3b307bf694fbba1d4f426d951861f3031084d3b8007bd519
User & Date: gawthrop@users.sourceforge.net on 2003-06-06 07:18:27
Other Links: branch diff | manifest | tags
Context
2003-06-09
10:49:23
Include error in out args check-in: 7337ff5fe9 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
2003-06-06
07:18:27
Include LQ PPP option
General tidy check-in: 17b8040493 user: gawthrop@users.sourceforge.net tags: origin/master, trunk
07:17:28
Removed debug check-in: f9152ecb6b user: gawthrop@users.sourceforge.net tags: origin/master, trunk
Changes
Hide Diffs Unified Diffs 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


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





  
  if nargin<2
    Simulate = 1;
  endif
  
  if nargin<3
    ControlType = 2;








>
>
>
>
>








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

    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,"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







  
  [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








>
|
|
|


















>
>
>
>
>
>
>






<
<
<
<
<
<








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,"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
  






  ## 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

    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

    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

    ## Checks

    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
  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)




  ## Control loop
  y = [];
  u = [];
  t = [];
  y_e = [];
  t_e = [];








>
|
|
>
>
>
>
>
>
>
>
|


>



<
<
<
<

|











>
>
>








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
    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





  ## Observer design
  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

      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);
    
				#       ## 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;








|








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,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 ]