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


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