- Integrated the file import into the main interface (not as a plugin anymore)
[mldemos:mldemos.git] / _AlgorithmsPlugins / SEDS / SEDS.cpp
1 #include "SEDS.h"\r
2 #ifndef M_PI\r
3 #define M_PI 3.14159265358979323846\r
4 #endif\r
5 \r
6 #include <QtGui>\r
7 \r
8 double NLOpt_Compute_J(unsigned nPar, const double *x, double *grad, void *f_data)\r
9 {\r
10     SEDS *seds = (SEDS *) f_data;\r
11 \r
12     Vector p(nPar),dJ(nPar);\r
13     p.Set(x,nPar);\r
14 \r
15     double J = seds->Compute_J(p, dJ);\r
16     for (int i=0; i<nPar; i++)\r
17         grad[i] = dJ[i];\r
18 \r
19     double J_tmp = 1e20;\r
20     if (seds->displayData.size()>0)\r
21         J_tmp = seds->displayData.back();\r
22 \r
23     J_tmp = min(J,J_tmp);\r
24     seds->displayData.push_back(J_tmp);\r
25         //seds->PaintData(seds->displayData);\r
26 \r
27     return J;\r
28 }\r
29 \r
30 void NLOpt_Constraint(unsigned nCtr, double *result, unsigned nPar, const double* x, double* grad, void* f_data)\r
31 {\r
32     SEDS *seds = (SEDS *) f_data;\r
33 \r
34     Vector c(nCtr);\r
35     Matrix dc(nCtr,nPar);\r
36     seds->Compute_Constraints(c, dc, false);\r
37 \r
38     for (int i=0; i<nCtr; i++){\r
39         result[i] = c[i];\r
40         for (int j=0; j<nPar; j++)\r
41             grad[i*nPar+j] = dc(i,j);\r
42     }\r
43 }\r
44 \r
45 //constructor\r
46 SEDS::SEDS()\r
47 {\r
48     Options.max_iter = 1000;\r
49     Options.tol_stopping = 1e-4;\r
50     Options.delta = 1.0e-5;\r
51     Options.tol_mat_bias = 1e-12;\r
52     Options.perior_opt = 1;\r
53     Options.mu_opt = 1;\r
54     Options.sigma_x_opt = 1;\r
55     Options.display = 1;\r
56     Options.objective = 1; //i.e. using likelihood\r
57     Options.eps_margin = 1e-4;\r
58     Options.SEDS_Ver = 2;\r
59     d = 0;\r
60     nData = 0;\r
61 }\r
62 \r
63 /* Parsing the input commands to the solver */\r
64 bool SEDS::Parse_Input(int argc, char **argv, char** file_data, char** file_model, char** file_output)\r
65 {\r
66     if ((argc-1)%2!=0 && argc > 1 && strcmp(argv[1],"-h")){\r
67         cout << "Improper number of input arguments!" << endl;\r
68         cout << "Leaving optimization.!" << endl;\r
69         return 0;\r
70     }\r
71 \r
72     for (int i=1; i < argc; i++){\r
73         if (!strcmp(argv[i],"-dfile")) //name of the data file to be passed to the solver\r
74             *file_data = argv[i+1];\r
75         else if (!strcmp(argv[i],"-mfile"))// name of the file contains the model's initial guess to be passed to the solver\r
76             *file_model = argv[i+1];\r
77         else if (!strcmp(argv[i],"-ofile"))// name of the file contains the model's initial guess to be passed to the solver\r
78             *file_output = argv[i+1];\r
79         else if (!strcmp(argv[i],"-i"))  // an integer defining the maximum number of iterations\r
80             Options.max_iter = atoi(argv[i+1]);\r
81         else if (!strcmp(argv[i],"-t")) // a double variable defining an additive bias term on Sigmas\r
82             Options.tol_mat_bias = atof(argv[i+1]);\r
83         else if (!strcmp(argv[i],"-s")) // a double variable defining the stopping threshold\r
84             Options.tol_stopping = atof(argv[i+1]);\r
85         else if (!strcmp(argv[i],"-o")){// defining the objective function\r
86             if (!strcmp(argv[i+1],"mse"))\r
87                 Options.objective = 0;\r
88         }\r
89         else if (!strcmp(argv[i],"-op"))// a 0 or 1 integer defining whether Prior should be optimized\r
90             Options.perior_opt = atoi(argv[i+1]) > 0;\r
91         else if (!strcmp(argv[i],"-om"))// a 0 or 1 integer defining whether Mu should be optimized\r
92             Options.mu_opt = atoi(argv[i+1]) > 0;\r
93         else if (!strcmp(argv[i],"-os"))// a 0 or 1 integer defining whether Sigma should be optimized\r
94             Options.sigma_x_opt = atoi(argv[i+1]) > 0;\r
95         else if (!strcmp(argv[i],"-d"))// a 0 or 1 integer defining whether to display results on the screen\r
96             Options.display = atoi(argv[i+1]) > 0;\r
97         else if (!strcmp(argv[i],"-h")){// displaying help\r
98 \r
99             cout << "\n SEDS optimization toolbox. This function finds an optimal value of" << endl;\r
100             cout << " a Gaussian Mixture Model under the constraint of ensuring its global" << endl;\r
101             cout << " asymptotic stability." << endl;\r
102             cout << " " << endl;\r
103             cout << " Syntax " << endl;\r
104             cout << "     (Working Folder)$ ./optimization_SEDS -dfile <datafile> " << endl;\r
105             cout << "                       -mfile <modelfile>  -ofile <outputfile>" << endl;\r
106             cout << "                        [optional commands]" << endl;\r
107             cout << " " << endl;\r
108             cout << " " << endl;\r
109             cout << " Inputs -----------------------------------------------------------------" << endl;\r
110             cout << " " << endl;\r
111             cout << "   o -dfile:  Data file contains demonstration datapoints." << endl;\r
112             cout << "              The package supports both binary and text formats." << endl;\r
113             cout << "              For binary files use the file extension .bin and for" << endl;\r
114             cout << "              text files use .txt" << endl;\r
115             cout << " " << endl;\r
116             cout << "              - If the data file is binary, the structure of the file is " << endl;\r
117             cout << "                     <d (int)> <nData (int)>" << endl;\r
118             cout << "                     <Data(1,:) array of nData (double)>" << endl;\r
119             cout << "                          ..." << endl;\r
120             cout << "                     <Data(2*d,:) array of nData (double)>" << endl;\r
121             cout << " " << endl;\r
122             cout << "              - If the file is in the text format, the structure of the file is " << endl;\r
123             cout << "                Each line has 2*d elements and corresponds to each datapoint" << endl;\r
124             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
125             cout << " " << endl;\r
126             cout << " " << endl;\r
127             cout << "   o -mfile:    Model file contains an initial guess for the model." << endl;\r
128             cout << "                The package supports both binary and text format." << endl;\r
129             cout << "                For binary files use the file extension .bin and for" << endl;\r
130             cout << "                text files use .txt" << endl;\r
131             cout << " " << endl;\r
132             cout << "              - If the model file is binary, the structure of the file is " << endl;\r
133             cout << "                     <d (int)> <K (int)> <Priors array of K (double)>" << endl;\r
134             cout << "                     <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>" << endl;\r
135             cout << "                     <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>" << endl;\r
136             cout << "                         ..." << endl;\r
137             cout << "                     <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>" << endl;\r
138             cout << " " << endl;\r
139             cout << "              - If the file is in the text format, the structure of the file is " << endl;\r
140             cout << "                     First Line:         d" << endl;\r
141             cout << "                     Second Line:         K" << endl;\r
142             cout << "                     Third Line:         Priors" << endl;\r
143             cout << "                     Next 2*d Lines:    Mu" << endl;\r
144             cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;\r
145             cout << "                         ..." << endl;\r
146             cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;\r
147             cout << " " << endl;\r
148             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
149             cout << " " << endl;\r
150             cout << "   o -ofile:    Name of the output file. The obtained optimal value for the GMM" << endl;\r
151             cout << "                will be saved as a text file based on the following order:" << endl;\r
152             cout << "                     First Line:         d" << endl;\r
153             cout << "                     Second Line:         K" << endl;\r
154             cout << "                     Third Line:         Priors" << endl;\r
155             cout << "                     Next 2*d Lines:    Mu" << endl;\r
156             cout << "                     Next 2*d Lines:    Sigma(:,:,1)" << endl;\r
157             cout << "                         ..." << endl;\r
158             cout << "                     Next 2*d Lines:    Sigma(:,:,K)" << endl;\r
159             cout << " " << endl;\r
160             cout << "                For more detailed information see the file 'Communicate2Exe.m'" << endl;\r
161             cout << " " << endl;\r
162             cout << " " << endl;\r
163             cout << " Optional Commands-------------------------------------------------------" << endl;\r
164             cout << "       -t:    a very small positive scalar to avoid instabilities in " << endl;\r
165             cout << "              Gaussian kernel [default: 10^-15]" << endl;\r
166             cout << " " << endl;\r
167             cout << "       -s:    A small positive scalar defining the stoppping tolerance for " << endl;\r
168             cout << "              the optimization solver [default: 10^-10]" << endl;\r
169             cout << " " << endl;\r
170             cout << "       -i:    maximum number of iteration for the solver [default: i_max=1000]" << endl;\r
171             cout << " " << endl;\r
172             cout << "       -p:    Most of the time, it is preferable to transform a constrained " << endl;\r
173             cout << "              optimization problem into an unconstrained one by penalizing " << endl;\r
174             cout << "              if the constrains are violated. 'cons_penalty' should be a " << endl;\r
175             cout << "              big value in comparison to the order of magnitude of data." << endl;\r
176             cout << "              If you wish to solve the real unconstrained problem, set the" << endl;\r
177             cout << "              value of 'cons_penalty' to Inf [default: 10^4]" << endl;\r
178             cout << " " << endl;\r
179             cout << "       -o:   'likelihood': use likelihood as criterion to optimize parameters " << endl;\r
180             cout << "              of GMM 'mse': use mean square error as criterion to optimize " << endl;\r
181             cout << "              parameters of GMM " << endl;\r
182             cout << " " << endl;\r
183             cout << "       -d:    An option to control whether the algorithm displays the output" << endl;\r
184             cout << "              of each iterations [default: true]" << endl;\r
185             cout << " " << endl;\r
186             cout << "       -op:   Shall the sover optimize priors? This is an option given to the" << endl;\r
187             cout << "              user if s/he wishes not to optimize the priors [default: true]" << endl;\r
188             cout << " " << endl;\r
189             cout << "       -om:   Shall the sover optimize centers? This is an option given to the" << endl;\r
190             cout << "              user if s/he wishes not to optimize the centers Mu [default: true]" << endl;\r
191             cout << " " << endl;\r
192             cout << "       -os:   Shall the sover optimize Sigma_x? This is an option given to the " << endl;\r
193             cout << "              user if s/he wishes not to optimize the Sigma_x [default: true]" << endl;\r
194             cout << " " << endl;\r
195             cout << " " << endl;\r
196             cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;\r
197             cout << "   %%%    Copyright (c) 2010 S. Mohammad Khansari-Zadeh, LASA Lab, EPFL,   %%%" << endl;\r
198             cout << "   %%%          CH-1015 Lausanne, Switzerland, http://lasa.epfl.ch         %%%" << endl;\r
199             cout << "   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%" << endl;\r
200             cout << " " << endl;\r
201             cout << "   The program is free for non-commercial academic use. Please contact the" << endl;\r
202             cout << "   author if you are interested in using the software for commercial purposes." << endl;\r
203             cout << "   The software must not be modified or distributed without prior permission" << endl;\r
204             cout << "   of the authors. Please acknowledge the authors in any academic publications" << endl;\r
205             cout << "   that have made use of this code or part of it. Please use this BibTex" << endl;\r
206             cout << "   reference:" << endl;\r
207             cout << " " << endl;\r
208             cout << "      S. M. Khansari Zadeh and A. Billard, 'Imitation learning of Globally " << endl;\r
209             cout << "      Stable Non-Linear Point-to-Point Robot Motions using Nonlinear" << endl;\r
210             cout << "      Programming', in Proceeding of the 2010 IEEE/RSJ International" << endl;\r
211             cout << "      Conference on Intelligent Robots and Systems (IROS 2010), Taipei," << endl;\r
212             cout << "      Taiwan, October 2010 " << endl;\r
213             cout << " " << endl;\r
214             cout << "    To get latest upadate of the software please visit" << endl;\r
215             cout << "                             http://lasa.epfl.ch/khansari" << endl;\r
216             cout << " " << endl;\r
217             cout << "    Please send your feedbacks or questions to:" << endl;\r
218             cout << "                             mohammad.khansari_at_epfl.ch\n\n" << endl;\r
219 \r
220             return 0;\r
221         }\r
222         else{\r
223             cout << "\nInvalid arguments '" << argv[i] <<"', please try again." << endl;\r
224             cout << "Use -h to get a list of possible commands." << endl;\r
225             cout << "Leaving optimization.!\n" << endl;\r
226             return 0;\r
227         }\r
228 \r
229         i++;\r
230     }\r
231 \r
232     if (*file_data == NULL){\r
233         cout << "\nThe name of the data file is not passed to the solver." << endl;\r
234         cout << "Use the option -dfile to specify the file name." << endl;\r
235         cout << "Leaving optimization.!\n" << endl;\r
236         return 0;\r
237     }\r
238     if (*file_model == NULL){\r
239         cout << "\nThe name of the model file is not passed to the solver." << endl;\r
240         cout << "Use the option -mfile to specify the file name." << endl;\r
241         cout << "Leaving optimization.!\n" << endl;\r
242         return 0;\r
243     }\r
244     if (*file_output == NULL){\r
245         cout << "\nThe name of the output file is not passed to the solver." << endl;\r
246         cout << "Use the option -ofile to specify the file name." << endl;\r
247         cout << "Leaving optimization.!\n" << endl;\r
248         return 0;\r
249     }\r
250 \r
251     return 1;\r
252 }\r
253 \r
254 /* Loading demonstration detapoint\r
255  * fileName: name of the file containing datapoints\r
256   * For binary files use the file extension .bin and for\r
257  * text files use .txt\r
258  *\r
259  *               - If the data file is binary, the structure of the file is\r
260  *                               <d (int)> <nData (int)> <Data(1,:) array of nData (double)> ... <Data(2*d,:) array of nData (double)>\r
261  *\r
262  *               - If the file is in the text format, the structure of the file is\r
263  *                               Each line has 2*d elements and corresponds to each datapoint\r
264  * For more detailed information see the file 'Communicate2Exe.m'\r
265 */\r
266 bool SEDS::loadData(const char fileName[], char type)\r
267 {\r
268     // Load coefficient of a GMM from a file\r
269 \r
270     if (type == 'b'){ //reading a binary file\r
271         //open the file\r
272         FILE *file = fopen(fileName, "rb");\r
273         if (!file)\r
274         {\r
275             std::cout << "Error: Could not open the file!" << std::endl;\r
276             return false;\r
277         }\r
278         //read from file, get nData etc\r
279         fread(&d, sizeof(int), 1, file);\r
280         fread(&nData, sizeof(int), 1, file);\r
281 \r
282         //read from file, get data matrix\r
283         Data.Resize(2*d,nData);\r
284         fread(Data.Array(), sizeof(REALTYPE), nData*2*d, file);\r
285         fclose(file);\r
286     }\r
287     else{\r
288         std::ifstream file(fileName);\r
289 \r
290         if(!file.is_open()){\r
291             std::cout << "Error: Could not open the file!" << std::endl;\r
292             return false;\r
293         }\r
294 \r
295         char tmp[1024];\r
296         double valTmp;\r
297         nData = 0;\r
298         d = 0;\r
299         // Get number of row\r
300         while(!file.eof()){\r
301             file.getline(tmp,1024);\r
302             nData++;\r
303             if (nData==1){\r
304                 // Get number of columns\r
305                 std::istringstream strm;\r
306                 strm.str(tmp);\r
307                 while (strm >> valTmp)\r
308                     d++;\r
309             }\r
310         }\r
311         nData = nData - 1;\r
312         d = d/2;\r
313         Data.Resize(2*d,nData); // note that Data' is saved in the text file\r
314         file.clear();\r
315         file.seekg(0); // returns to beginning of the file\r
316         for(unsigned int i=0;i<nData;i++){\r
317             file.getline(tmp,1024);\r
318             std::istringstream strm;\r
319             strm.str(tmp);\r
320             for(unsigned int j=0;j<2*d;j++){\r
321                 strm >> Data(j,i);\r
322             }\r
323         }\r
324         file.close();\r
325     }\r
326     return true;\r
327 }\r
328 \r
329 /* Loading initial guess of the model\r
330  * fileName: name of the file containing the model\r
331  * For binary files use the file extension .bin and for\r
332  * text files use .txt\r
333  *\r
334  *       - If the model file is binary, the structure of the file is\r
335  *       <d (int)> <K (int)> <Priors array of K (double)>\r
336  *               <Mu(1,:) array of K (double)> ... <Mu(2*d,:) array of K (double)>\r
337  *               <Sigma(1,:,1) array of 2*d (double)> ... <Sigma(2*d,:,1) array of 2*d (double)>\r
338  *                        ...\r
339  *               <Sigma(1,:,K) array of 2*d (double)> ... <Sigma(2*d,:,K) array of 2*d (double)>\r
340  *\r
341  *       - If the file is in the text format, the structure of the file is\r
342  *               First Line:            d\r
343  *               Second Line:           K\r
344  *               Third Line:            Priors\r
345  *               Next 2*d Lines:        Mu\r
346  *               Next 2*d Lines:        Sigma(:,:,1)\r
347  *                       ...\r
348  *               Next 2*d Lines:        Sigma(:,:,K)\r
349  *\r
350  * For more detailed information see the file 'Communicate2Exe.m'\r
351 */\r
352 bool SEDS::loadModel(const char fileName[], char type)\r
353 {\r
354     // Load coefficient of a GMM from a file\r
355 \r
356     if (type == 'b'){ //reading a binary file\r
357         //open the file\r
358         FILE *file = fopen(fileName, "rb");\r
359         if (!file)\r
360         {\r
361             std::cout << "Error: Could not open the file!" << std::endl;\r
362             return false;\r
363         }\r
364         //read from file, get d and K etc\r
365         fread(&d, sizeof(int), 1, file);\r
366         fread(&K, sizeof(int), 1, file);\r
367 \r
368         d /= 2;\r
369 \r
370         //read prior\r
371         Priors.Resize(K);\r
372         fread(Priors.Array(), sizeof(REALTYPE), K, file);\r
373 \r
374         //read Mu\r
375         Mu.Resize(2*d,K);\r
376         fread(Mu.Array(), sizeof(REALTYPE), 2*d*K, file);\r
377 \r
378         //read Sigma\r
379         Sigma = new Matrix[K];\r
380         for (int k = 0; k < K; k++)\r
381         {\r
382             Sigma[k] = Matrix(2*d,2*d);\r
383             fread(Sigma[k].Array(), sizeof(REALTYPE), 4*d*d, file);\r
384         }\r
385 \r
386         fclose(file);\r
387     }\r
388     else{\r
389         std::ifstream file(fileName);\r
390 \r
391         if(!file.is_open()){\r
392             std::cout << "Error: Could not open the file!" << std::endl;\r
393             return false;\r
394         }\r
395 \r
396         file >> d >> K;\r
397 \r
398         d /= 2;\r
399 \r
400         Priors.Resize(K);\r
401         for (int k = 0; k < K; k++)\r
402             file >> Priors[k];\r
403 \r
404         Mu.Resize(2*d,K);\r
405         for (int k = 0; k < K; k++)\r
406             for (int i = 0; i < 2*d; i++)\r
407                 file >> Mu(i,k);\r
408 \r
409         Sigma = new Matrix[K];\r
410         for (int k = 0; k < K; k++)\r
411         {\r
412             Sigma[k] = Matrix(2*d,2*d);\r
413             for (int i = 0; i < 2*d; i++)\r
414                 for (int j = 0; j < 2*d; j++)\r
415                     file >> Sigma[k](i,j);\r
416         }\r
417         file.close();\r
418     }\r
419 \r
420     return true;\r
421 }\r
422 \r
423 /* Saving the optimal obtained model from SEDS\r
424  * fileName: name of the file to save the model.\r
425  * The model will be saved in text format based on the following strcuture:\r
426  *\r
427  *                               First Line:            d\r
428  *                               Second Line:           K\r
429  *                               Third Line:            Priors\r
430  *                               Next 2*d Lines:        Mu\r
431  *                               Next 2*d Lines:        Sigma(:,:,1)\r
432  *                               ...\r
433  *                               Next 2*d Lines:        Sigma(:,:,K)\r
434  *\r
435  * For more detailed information see the file 'Communicate2Exe.m'\r
436 */\r
437 bool SEDS::saveModel(const char fileName[])\r
438 {\r
439     // Save the dataset to a file\r
440     std::ofstream file(fileName);\r
441 \r
442     if(!file){\r
443         std::cout << "Error: Could not open the file!" << std::endl;\r
444         return false;\r
445     }\r
446 \r
447     file << d << endl; //dimension\r
448     file << K << endl << endl; //number of Gaussian\r
449 \r
450     file.precision(10); //setting the precision of writing\r
451 \r
452     for (unsigned int k = 0; k < K; k++)\r
453         file << Priors(k) << " ";\r
454     file << endl << endl;\r
455 \r
456     for (unsigned int i = 0; i < 2*d; i++){\r
457         for (unsigned int k = 0; k < K; k++)\r
458             file << Mu(i,k) << " ";\r
459         file << endl;\r
460     }\r
461     file << endl;\r
462 \r
463     for (unsigned int k = 0; k < K; k++){\r
464         for (unsigned int i = 0; i < 2*d; i++){\r
465             for (unsigned int j = 0; j < 2*d; j++)\r
466                 file << Sigma[k](i,j) << " ";\r
467             file << endl;\r
468         }\r
469         file << endl;\r
470     }\r
471     file.close();\r
472 \r
473     return true;\r
474 }\r
475 \r
476 void SEDS::preprocess_sigma(){\r
477     for (int k=0; k<K; k++)\r
478     {\r
479         for (int i=0; i<d; i++)\r
480         {\r
481             for (int j=0; j<d; j++)\r
482             {\r
483                 if(i==j)\r
484                 {\r
485                     Sigma[k](i,i) = fabs(Sigma[k](i,i));\r
486                     Sigma[k](i+d,i) = -fabs(Sigma[k](i+d,i));\r
487                     Sigma[k](i,i+d) = -fabs(Sigma[k](i,i+d));\r
488                     Sigma[k](i+d,i+d) = fabs(Sigma[k](i+d,i+d));\r
489                 }\r
490                 else\r
491                 {\r
492                     Sigma[k](i,j) = 0.;\r
493                     Sigma[k](i+d,j) = 0.;\r
494                     Sigma[k](i,j+d) = 0.;\r
495                     Sigma[k](i+d,j+d) = 0.;\r
496                 }\r
497             }\r
498         }\r
499     }\r
500 }\r
501 \r
502 bool SEDS::initialize_value(){\r
503     tmpData = new Matrix[K];\r
504     Pxi = new Vector[K];\r
505     h = new Vector[K];\r
506     h_tmp = new Vector[K];\r
507     prob.Resize(nData);\r
508     Pxi_Priors.Resize(nData); //a vector representing Pxi*Priors\r
509 \r
510     A = new Matrix[K];\r
511     invSigma_x = new Matrix[K];\r
512     Sigma_xdx = new Matrix[K];\r
513     Mu_x = new Vector[K];\r
514     Mu_xd = new Vector[K];\r
515     invSigma = new Matrix[K];\r
516     B_Inv = new Matrix[d];\r
517 \r
518     detSigma.Resize(K);\r
519     detSigma_x.Resize(K);\r
520     rArs.Resize(d,d);\r
521     rBrs.Resize(d,d);\r
522     rAvrs.Resize(d); //vector form of rArs!!!\r
523     tmp_A.Resize(d,2*d); //will be used for eq to Matlab [eye(2);A(:,:,i)]\r
524     B.Resize(d,d);\r
525 \r
526     sum_dp.Resize(K);\r
527 \r
528     if (Options.objective)\r
529         tmp_mat.Resize(2*d,nData);\r
530     else\r
531         tmp_mat.Resize(d,nData);\r
532 \r
533     for(int k=0; k<K; k++) {\r
534         Pxi[k] = Vector(nData);\r
535         h[k] = Vector(nData);\r
536         h_tmp[k] = Vector(nData);\r
537         invSigma_x[k]=Matrix(d,d);\r
538         Sigma_xdx[k]=Matrix(d,d);\r
539         invSigma[k]=Matrix(2*d,2*d);\r
540         A[k]=Matrix(d,d);\r
541         Mu_x[k] = Vector(d);\r
542         Mu_xd[k] = Vector(d);\r
543     }\r
544 \r
545     if (Options.objective){ //likelihood objective function\r
546         rSrs.Resize(2*d,2*d);\r
547         L = new Matrix[K];\r
548         for(int k=0; k<K; k++){\r
549             tmpData[k] = Matrix(2*d,nData);\r
550             L[k] = Matrix(2*d,2*d);\r
551         }\r
552     }else{ //mse objective function\r
553         rSrs.Resize(d,d);\r
554         Sigma_x = new Matrix[K];\r
555         L_x = new Matrix[K];\r
556         X = Data.GetMatrix(0,d-1,0,nData-1);\r
557         Xd = Data.GetMatrix(d,2*d-1,0,nData-1);\r
558         Xd_hat.Resize(d,nData);\r
559         for(int k=0; k<K; k++){\r
560             tmpData[k] = Matrix(d,nData);\r
561             L_x[k] = Matrix(d,d);\r
562             Sigma_x[k] = Matrix(d,d);\r
563         }\r
564     }\r
565 \r
566     C_Lyapunov.Resize(d,d);\r
567     for (int i=0;i<d;i++)\r
568         C_Lyapunov(i,i) = 1;\r
569     return true;\r
570 }\r
571 \r
572 /*\r
573 QPixmap pm(320,240);\r
574 QLabel lbl;\r
575 void SEDS::PaintData(std::vector<float> data)\r
576 {\r
577     QPainter painter(&pm);\r
578     painter.fillRect(pm.rect(), Qt::white);\r
579 \r
580     int w = pm.width();\r
581     int h = pm.height();\r
582     int cnt = data.size();\r
583     int pad = 10;\r
584     QPointF oldPoint;\r
585     double minVal = FLT_MAX;\r
586     double maxVal = -FLT_MAX;\r
587     for(int i=0; i< data.size(); i++)\r
588     {\r
589         if(minVal > data[i]) minVal = data[i];\r
590         if(maxVal < data[i]) maxVal = data[i];\r
591     }\r
592     if (minVal == maxVal)\r
593     {\r
594         minVal = 0;\r
595     }\r
596 \r
597     painter.setBrush(Qt::NoBrush);\r
598     painter.setPen(QPen(QColor(200,200,200), 0.5));\r
599     int steps = 10;\r
600     for(int i=0; i<=steps; i++)\r
601     {\r
602         painter.drawLine(QPoint(0, i/(float)steps*(h-2*pad) + pad), QPoint(w, i/(float)steps*(h-2*pad) + pad));\r
603         painter.drawLine(QPoint(i/(float)steps*w, 0), QPoint(i/(float)steps*w, h));\r
604     }\r
605     painter.setRenderHint(QPainter::Antialiasing);\r
606 \r
607     painter.setPen(QPen(Qt::black, 1.5));\r
608     for(int i=0; i< data.size(); i++)\r
609     {\r
610         float value = data[i];\r
611         if (value != value) continue;\r
612         float x = i/(float)cnt*w;\r
613         float y = (1 - (value-minVal)/(maxVal - minVal)) * (float)(h-2*pad) + pad;\r
614         QPointF point(x, y);\r
615         if(i) painter.drawLine(oldPoint, point);\r
616         //painter.drawEllipse(point, 3, 3);\r
617         oldPoint = point;\r
618     }\r
619     painter.setPen(QPen(Qt::black, 0.5));\r
620     painter.setBrush(QColor(255,255,255,200));\r
621     painter.drawRect(QRect(190,5,100,45));\r
622     painter.setPen(QPen(Qt::black, 1));\r
623     painter.drawText(QPointF(200, 20), QString("J_0: %1").arg(data[0]));\r
624     painter.drawText(QPointF(200, 40), QString("J_F: %1").arg(data[data.size()-1]));\r
625     lbl.setPixmap(pm);\r
626     lbl.show();\r
627     qApp->processEvents(QEventLoop::ExcludeUserInputEvents);\r
628 }\r
629 */\r
630 \r
631 /* Running optimization solver to find the optimal values for the model.\r
632  * The result will be saved in the variable p\r
633 */\r
634 bool SEDS::Optimize(){\r
635     displayData.clear();\r
636     /*string str = "test.txt";\r
637     loadModel(str.c_str());\r
638     str = "data.txt";\r
639     loadData(str.c_str());*/\r
640     initialize_value();\r
641     preprocess_sigma();\r
642 \r
643     if (K==1)\r
644         Options.perior_opt = false;\r
645 \r
646     if (Options.objective){\r
647         nPar = Options.perior_opt*K + Options.mu_opt*K*d + Options.sigma_x_opt*K*d*(d+1) + K*d*d + (Options.SEDS_Ver-1)*d*d;\r
648         p.Resize(nPar); //a vector of parameters\r
649         GMM_2_Parameters_Likelihood(p);\r
650     }else{\r
651         nPar = Options.perior_opt*K + Options.mu_opt*K*d + Options.sigma_x_opt*K*d*(d+1)/2 + K*d*d + (Options.SEDS_Ver-1)*d*d;\r
652         p.Resize(nPar); //a vector of parameters\r
653         GMM_2_Parameters_MSE(p);\r
654     }\r
655     nCtr = K*d + (Options.SEDS_Ver-1)*d;\r
656 \r
657     //-running NLOpt--------------------------------------------------------------------------------------\r
658     //double lb[2] = { -HUGE_VAL, 0 }; // lower bounds\r
659     nlopt::opt opt(nlopt::LD_MMA, nPar); // algorithm and dimensionality\r
660     //nlopt_set_lower_bounds(opt, lb);\r
661     opt.set_min_objective(NLOpt_Compute_J, this);\r
662 \r
663     //nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);\r
664     //nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);\r
665 \r
666     opt.set_xtol_rel(Options.tol_stopping);\r
667     opt.set_maxeval(Options.max_iter);\r
668 \r
669     std::vector<double> vec_tol(nCtr);\r
670     for (int i=0; i<nCtr; i++)\r
671         vec_tol[i] = Options.tol_stopping;\r
672 \r
673     opt.add_inequality_mconstraint(NLOpt_Constraint, this, vec_tol);\r
674 \r
675     double minf; // the minimum objective value, upon return\r
676 \r
677     std::vector<double> p_std(nPar);\r
678     for (int i=0; i<nPar; i++)\r
679         p_std[i] = p[i];\r
680 \r
681     double min0 = Compute_J(p);\r
682 \r
683     int nlopt_result = opt.optimize(p_std, minf);\r
684     if ( nlopt_result < 0) {\r
685         printf("nlopt failed!\n");\r
686     }\r
687     else {\r
688         for (int i=0; i<nPar; i++)\r
689             p[i] = p_std[i];\r
690         printf("Decrease the objective function from %0.10g to %0.10g\n", min0, minf);\r
691     }\r
692     printf("exit flag = %d\n", nlopt_result);\r
693     //--------------------------------------------------------------------------------------------\r
694 \r
695 \r
696     //forming Priors, Mu, and Sigma from parameters p\r
697     if (Options.objective)\r
698         Parameters_2_GMM_Likelihood(p);\r
699     else{\r
700         Parameters_2_GMM_MSE(p);\r
701         Priors /= Priors.Sum();\r
702         for (int k=0;k<K;k++){\r
703             for (int i=0;i<d;i++){\r
704                 for (int j=0;j<d;j++){\r
705                     Sigma[k](i,j) = Sigma_x[k](i,j);\r
706                     Sigma[k](i+d,j) = Sigma_xdx[k](i,j);\r
707                     Sigma[k](j,i+d) = Sigma_xdx[k](j,i);\r
708                 }\r
709             }\r
710         }\r
711     }\r
712 \r
713     CheckConstraints(A);\r
714 }\r
715 \r
716 /* This function computes the sensitivity of Cost function w.r.t. optimization parameters.\r
717  * The result is saved in the Vector dJ. The returned value of function is J.\r
718  * Don't mess with this function. Very sensitive and a bit complicated!\r
719 */\r
720 double SEDS::Compute_J(Vector pp, Vector& dJ) //compute the objective function and derivative w.r.t parameters (updated in vector dJ)\r
721 {\r
722     double J = Compute_J(pp); //computing the cost function\r
723 \r
724     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
725     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
726     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
727 \r
728 \r
729     for(int i=0; i<d; i++)\r
730         tmp_A(i,i) = 1;\r
731 \r
732     double det_term;\r
733     double term,sum;\r
734     Vector dJ_dMu_k(d);\r
735     int ind_start,ind_max_col;\r
736 \r
737     if (Options.sigma_x_opt){\r
738         ind_start = 0;\r
739         ind_max_col = 2*d;\r
740     }else{\r
741         ind_start = d;\r
742         ind_max_col = d;\r
743     }\r
744 \r
745     dJ.Zero();\r
746     for(int k=0; k<K; k++){\r
747         //sensitivity wrt Priors\r
748         if (Options.perior_opt && Options.objective){ //likelihood\r
749             dJ[k] = -exp(-pp[k])*Priors[k]*sum_dp[k];\r
750             /*\r
751             h[k] = Pxi[k]*Priors[k]/Pxi_Priors;\r
752             dJ(k)=-exp(pp(k))/Priors[k]*((h[k]-Priors[k]).Sum());\r
753             */\r
754         }else if (!Options.objective){\r
755             sum = 0;\r
756             double tmp_dbl;\r
757             h_tmp[k].Zero();\r
758             tmp_mat = A[k]*X;\r
759             REALTYPE *p_tmp_mat = tmp_mat.Array();\r
760             REALTYPE *p_Xd_hat = Xd_hat.Array();\r
761             REALTYPE *p_Xd = Xd.Array();\r
762 \r
763             for (int i=0; i<d; i++){\r
764                 REALTYPE *p_h_tmp = h_tmp[k].Array();\r
765                 REALTYPE *p_h = h[k].Array();\r
766                 for (int j=0; j<nData; j++){\r
767                     tmp_dbl = *(p_h++) * (*(p_tmp_mat++)-*p_Xd_hat) * (*(p_Xd_hat++)-*(p_Xd++));\r
768                     *(p_h_tmp++) += tmp_dbl;\r
769                     sum += tmp_dbl;\r
770                 }\r
771             }\r
772             if (Options.perior_opt)\r
773                 dJ[k] = exp(-pp[k])*Priors[k]*sum;\r
774                 /*\r
775                 h_tmp[k] = h[k]^(((A[k]*X-Xd_hat)^(Xd_hat-Xd)).SumRow()); //This vector is common in all dJ computation.\r
776                 Thus, I defined it as a variable to save some computation power\r
777                 dJ(k)= h_tmp[k].Sum();  //derivative of priors(k) w.r.t. p(k)\r
778                 */\r
779         }\r
780 \r
781         if (Options.mu_opt)\r
782         {\r
783             if (Options.objective){ //likelihood\r
784                 tmp_A.InsertSubMatrix(0,d,A[k].Transpose(),0,d,0,d); // eq to Matlab [eye(2) A(:,:,i)']\r
785                 dJ_dMu_k=-((tmp_A*invSigma[k])*tmpData[k])*h[k];\r
786                 dJ.InsertSubVector(counter_mu,dJ_dMu_k,0,d);\r
787                 counter_mu += d;\r
788             }\r
789             else{ //mse\r
790                 tmp_mat = invSigma_x[k]*tmpData[k];\r
791                 REALTYPE *p_tmp_mat = tmp_mat.Array();\r
792                 REALTYPE *p_dJ = &dJ(counter_mu);\r
793 \r
794                 for (int i=0; i<d; i++){\r
795                     REALTYPE *p_h_tmp = h_tmp[k].Array();\r
796                     for (int j=0; j<nData; j++)\r
797                         *p_dJ += *(p_tmp_mat++) * (*(p_h_tmp++));\r
798                     p_dJ++;\r
799                     counter_mu++;\r
800                 }\r
801 \r
802                 /*\r
803                 dJ_dMu_k = (tmpData[k]*invSigma_x[k]).Transpose()*h_tmp[k];\r
804                 dJ.InsertSubVector(K+k*d,dJ_dMu_k,0,d);\r
805                 */\r
806             }\r
807         }\r
808 \r
809         //sensitivity wrt sigma\r
810         if (Options.objective) //likelihood\r
811             det_term = (detSigma(k)<0) ? -1:1; //det_term=sign(det_term)\r
812         else\r
813             det_term = (detSigma_x(k)<0) ? -1:1; //det_term=sign(det_term)\r
814 \r
815         if (Options.objective){ //likelihood\r
816             for (int i=0;i<ind_max_col;i++){\r
817                 int j = (Options.sigma_x_opt) ? i:d;\r
818                 while (j<2*d){\r
819                     rSrs.Zero();\r
820                     rSrs(j,i)=1;\r
821                     rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();\r
822 \r
823                     rAvrs = (-A[k] * rSrs.GetMatrix(0,d-1,0,d-1)+ rSrs.GetMatrix(d,2*d-1,0,d-1))*invSigma_x[k] * Mu_x[k];\r
824                     tmp_mat = (invSigma[k]*(rSrs*invSigma[k]))*tmpData[k];\r
825                     double tmp_dbl = (-0.5)*det_term*(invSigma[k]*rSrs).Trace();\r
826                     Vector tmp_vec = invSigma[k].GetMatrix(0,2*d-1,d,2*d-1)*rAvrs;\r
827 \r
828                     REALTYPE *p_tmp_mat = tmp_mat.Array();\r
829                     REALTYPE *p_tmpData = tmpData[k].Array();\r
830                     sum = 0;\r
831 \r
832                     for (int i=0; i<2*d; i++){\r
833                         REALTYPE *p_h = h[k].Array();\r
834                         for (int j=0; j<nData; j++){\r
835                             sum -= (0.5 * (*p_tmp_mat++) * (*p_tmpData) +\r
836                                     (i==0)*tmp_dbl + //derivative with respect to det Sigma which is in the numenator\r
837                                     (*p_tmpData++) * tmp_vec[i]) * (*p_h++); //since Mu_xi_d = A*Mu_xi, thus we should consider its effect here\r
838                         }\r
839                     }\r
840                     dJ(counter_sigma) = sum;\r
841                     counter_sigma++;\r
842                     j++;\r
843 \r
844                     /*\r
845                     Vector tmp_vec = (((invSigma[k]*(rSrs*invSigma[k]))*tmpData[k])^tmpData[k]).SumRow();\r
846 \r
847                     dJ(K+K*d+k*d*(2*d+1)+i_c-1) = ( tmp_vec*0.5 +\r
848                                                   (-0.5)*det_term*(invSigma[k]*rSrs).Trace() + //derivative with respect to det Sigma which is in the numenator\r
849                                                   tmpData[k].Transpose()*(invSigma[k].GetMatrix(0,2*d-1,d,2*d-1)*rAvrs)) //since Mu_xi_d = A*Mu_xi, thus we should consider its effect here\r
850                                                   *h[k]*(-1);\r
851                     */\r
852                 }\r
853             }\r
854         }else{ //mse\r
855             for (int i=0;i<d;i++){\r
856                 for (int j=0;j<d;j++){\r
857                     if (Options.sigma_x_opt && j>=i){\r
858                         rSrs.Zero();\r
859                         rSrs(j,i)=1;\r
860                         rSrs = rSrs*L_x[k].Transpose() + L_x[k]*rSrs.Transpose();\r
861 \r
862                         tmp_mat = (invSigma_x[k]*rSrs*invSigma_x[k])*tmpData[k];\r
863                         double tmp_dbl = -(invSigma_x[k]*rSrs).Trace();\r
864 \r
865                         REALTYPE *p_tmp_mat = tmp_mat.Array();\r
866                         REALTYPE *p_tmpData = tmpData[k].Array();\r
867                         sum = 0;\r
868 \r
869                         for (int ii=0; ii<d; ii++){\r
870                             REALTYPE *p_h_tmp = h_tmp[k].Array();\r
871                             for (int jj=0; jj<nData; jj++){\r
872                                 sum +=  (*p_h_tmp++) * ((*p_tmp_mat++) * (*p_tmpData++) //derivative w.r.t. Sigma in exponential\r
873                                                               + (ii == 0)*tmp_dbl); //derivative with respect to det Sigma which is in the numenator\r
874 \r
875                                 //the above term (i==0) is just to sum temp_dbl once\r
876                             }\r
877                         }\r
878                         dJ(counter_sigma) = 0.5*sum;\r
879                         counter_sigma++;\r
880 \r
881                         /*\r
882                         dJ(K+K*d+k*d*(2*d+1)+i_c-1) =  0.5*(\r
883                                                         ((((invSigma_x[k]*rSrs*invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow()  + //derivative w.r.t. Sigma in exponential\r
884                                                         -det_term*(invSigma_x[k]*rSrs.GetMatrix(0,d-1,0,d-1)).Trace())^h_tmp[k]/2) //derivative with respect to det Sigma which is in the numenator\r
885                                                          ).Sum();\r
886                         */\r
887                     }\r
888 \r
889                     sum = 0;\r
890                     rSrs.Zero();\r
891                     rSrs(j,i)=1;\r
892                     tmp_mat = rSrs*X;\r
893                     REALTYPE *p_tmp_mat = tmp_mat.Array();\r
894                     REALTYPE *p_Xd_hat = Xd_hat.Array();\r
895                     REALTYPE *p_Xd = Xd.Array();\r
896 \r
897                     for (int ii=0; ii<d; ii++){\r
898                         REALTYPE *p_h = h[k].Array();\r
899                         for (int jj=0; jj<nData; jj++){\r
900                             sum += *(p_h++) * (*(p_tmp_mat++)) * (*(p_Xd_hat++) - *(p_Xd++));\r
901                         }\r
902                     }\r
903                     dJ(counter_A) = sum;\r
904                     counter_A++;\r
905                     // dJ(counter_A) = sum(sum((rSrs*x).*dJdxd).*h(:,k)');  %derivative of A\r
906 \r
907                 }\r
908             }\r
909         }\r
910     }\r
911 \r
912     dJ /= nData;\r
913     return J;\r
914 }\r
915 \r
916 /* This function computes the sensitivity of Cost function w.r.t. optimization parameters.\r
917  * The result is saved in the Vector dJ. The returned value of function is J.\r
918  * Don't mess with this function. Very sensitive and a bit complicated!\r
919 */\r
920 double SEDS::Compute_J(Vector pp){\r
921 \r
922     double J = 0;\r
923     if (Options.objective){\r
924         Parameters_2_GMM_Likelihood(pp);\r
925     }else{\r
926         Parameters_2_GMM_MSE(pp);\r
927     }\r
928 \r
929     //computing likelihood:\r
930     Pxi_Priors.Zero();\r
931 \r
932 \r
933     for (int k=0; k<K; k++){\r
934         int d_tmp;\r
935         double tmp_den;\r
936         REALTYPE *p_X;\r
937 \r
938         if (Options.objective){ //likelihod\r
939             tmp_den = sqrt(pow(2*M_PI,2*d)*fabs(detSigma[k])+DBL_MIN);\r
940             d_tmp = 2*d;\r
941             p_X = Data.Array();\r
942         }\r
943         else{ //mse\r
944             tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);\r
945             d_tmp = d;\r
946             p_X = X.Array();\r
947         }\r
948 \r
949         REALTYPE *p_tmpData = tmpData[k].Array();\r
950         for(int j=0; j<d_tmp; j++){\r
951             double tmp_dbl = Mu(j,k);\r
952             for(int i=0; i<nData; i++)\r
953                 *p_tmpData++ = (*p_X++) - tmp_dbl;\r
954         }\r
955 \r
956         if (Options.objective){ //likelihod\r
957             tmp_mat = invSigma[k]*tmpData[k];\r
958         }\r
959         else{ //mse\r
960             tmp_mat = invSigma_x[k]*tmpData[k];\r
961         }\r
962 \r
963         REALTYPE *p_tmp_mat = tmp_mat.Array();\r
964         REALTYPE *p_Pxi = Pxi[k].Array();\r
965         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
966         p_tmpData = tmpData[k].Array();\r
967         prob.Zero();\r
968 \r
969         for(int i=0; i<d_tmp; i++){\r
970             REALTYPE *p_prob = prob.Array();\r
971             for(int j=0; j<nData; j++){\r
972                 if (i<d_tmp-1){\r
973                     *p_prob++ += (*p_tmp_mat++) * (*p_tmpData++);\r
974                 }\r
975                 else{\r
976                     *p_prob += (*p_tmp_mat++) * (*p_tmpData++);\r
977                     *p_Pxi = exp(-0.5*(*p_prob++))/tmp_den;\r
978                     *(p_Pxi_Priors++) += (*p_Pxi++)*Priors[k];\r
979                 }\r
980             }\r
981         }\r
982         /*\r
983         for(int j=0; j<d; j++)\r
984             tmpData[k].SetRow(Mu(j,k),j);\r
985 \r
986         tmpData[k]=X-tmpData[k]; // remove centers from data\r
987 \r
988         prob = ((invSigma_x[k]*tmpData[k])^tmpData[k]).SumRow();//cf the expontential in gaussian pdf\r
989 \r
990         double tmp_den = sqrt(pow(2*M_PI,d)*fabs(detSigma_x[k])+DBL_MIN);\r
991 \r
992         for(int j=0; j<nData; j++){\r
993             Pxi[k][j] = exp(-0.5*prob[j])/tmp_den;\r
994             Pxi_Priors[j] += Pxi[k][j]*Priors[k];\r
995         }\r
996         */\r
997     }\r
998 \r
999     //computing GMR\r
1000     if (Options.objective){ //likelihood\r
1001         for (int k=0; k<K; k++){\r
1002             REALTYPE *p_h = h[k].Array();\r
1003             REALTYPE *p_Pxi = Pxi[k].Array();\r
1004             REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1005             sum_dp[k] = 0;\r
1006 \r
1007             for (int j=0; j<nData; j++){\r
1008                 *p_h = (*p_Pxi++)/(*p_Pxi_Priors++)*Priors[k];\r
1009                 sum_dp[k] += (*p_h++) - Priors[k];\r
1010             }\r
1011         }\r
1012     }else{\r
1013         for (int k=0; k<K; k++){\r
1014             tmp_mat = A[k]*X;\r
1015             REALTYPE *p_tmp_mat = tmp_mat.Array();\r
1016             REALTYPE *p_Xd_hat = Xd_hat.Array();\r
1017             REALTYPE *p_Pxi = Pxi[k].Array();\r
1018             REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1019 \r
1020             for(int i=0; i<d; i++)\r
1021             {\r
1022                 REALTYPE *p_h = h[k].Array();\r
1023 \r
1024                 for(int j=0; j<nData; j++){\r
1025                     if (i==0)\r
1026                         *p_h = *(p_Pxi++)/(*(p_Pxi_Priors++)) * Priors[k];\r
1027                     if (k==0)\r
1028                         *(p_Xd_hat++) = *(p_h++) * (*p_tmp_mat++);\r
1029                     else\r
1030                         *(p_Xd_hat++) += *(p_h++) * (*p_tmp_mat++);\r
1031                 }\r
1032             }\r
1033 \r
1034             /*\r
1035             h[k] = Pxi[k]*Priors[k]/Pxi_Priors;\r
1036             if (k==0)\r
1037                 Xd_hat = Vector_Multiply(tmp_mul,h[k])^(A[k]*X);\r
1038             else\r
1039                 Xd_hat += Vector_Multiply(tmp_mul,h[k])^(A[k]*X);\r
1040             */\r
1041         }\r
1042     }\r
1043 \r
1044     //calc J\r
1045     if (Options.objective) {//likelihood\r
1046         REALTYPE *p_Pxi_Priors = Pxi_Priors.Array();\r
1047         for(int i=0; i<nData ; i++)\r
1048             J -= log(*p_Pxi_Priors++);\r
1049     }\r
1050     else{\r
1051         REALTYPE *p_Xd_hat = Xd_hat.Array();\r
1052         REALTYPE *p_Xd = Xd.Array();\r
1053         for(int i=0; i<d ; i++)\r
1054             for(int j=0; j<nData ; j++)\r
1055                 J += 0.5 * ((*p_Xd_hat) - (*p_Xd)) * ((*p_Xd_hat++) - (*p_Xd++));\r
1056         //J = 0.5*((Xd_hat-Xd)^(Xd_hat-Xd)).Sum();\r
1057     }\r
1058     J /= nData;\r
1059     return J;\r
1060 }\r
1061 \r
1062 /* Computing the stability constraints and their derivatives */\r
1063 void SEDS::Compute_Constraints(Vector &c, Matrix &dc, bool used_for_penalty){\r
1064     int i1_tmp;\r
1065     double tmp_detB, term;\r
1066     int counter_sigma = Options.perior_opt*K + Options.mu_opt*K*d; //the index at which sigma should start\r
1067     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1068     int counter_C = counter_A + K*d*d; //the index at which C_Lyapunov should start\r
1069     if (Options.objective) //likelihood\r
1070         counter_C += Options.sigma_x_opt*K*d*(d+1)/2; //because likelihood has more variable than mse\r
1071 \r
1072     c.Zero();\r
1073     dc.Zero();\r
1074 \r
1075     int ind_start,ind_max_col;\r
1076     if (Options.sigma_x_opt){\r
1077         ind_start = 0;\r
1078         ind_max_col = 2*d;\r
1079     }else{\r
1080         ind_start = d;\r
1081         ind_max_col = d;\r
1082     }\r
1083 \r
1084     if (Options.constraintCriterion){ //Principal Minor\r
1085         //constraints\r
1086         for (int k=0; k<K; k++)//for all states\r
1087         {\r
1088             i1_tmp = 1;\r
1089             B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov; //define B\r
1090 \r
1091             //computing the constraints (this part is basicly an exact rewrite of the matlab version)\r
1092             for (int i=0; i<d; i++) //for all dimensions\r
1093             {\r
1094                 B_Inv[i]=B.GetMatrix(0,i,0,i).Inverse(&tmp_detB);//get inverse and determinants of minors\r
1095 \r
1096                 if (!used_for_penalty || i1_tmp*tmp_detB+Options.eps_margin > 0)\r
1097                     c(k*d+i) = i1_tmp*tmp_detB + Options.eps_margin;\r
1098 \r
1099                 //computing the sensitivity of the constraints to the parameters\r
1100                 if (Options.objective){ //likelihood\r
1101                     int i_c = Options.sigma_x_opt*k*d*(d+1) + k*d*d;\r
1102                     for (int ii=0;ii<ind_max_col;ii++){\r
1103                         int jj = (Options.sigma_x_opt) ? ii:d;\r
1104                         while (jj<2*d){\r
1105                             rSrs.Zero();\r
1106                             rSrs(jj,ii) = 1;\r
1107                             rSrs = rSrs*L[k].Transpose() + L[k]*rSrs.Transpose();\r
1108                             rArs = (-A[k] * rSrs.GetMatrix(0,d-1,0,d-1) + rSrs.GetMatrix(d,2*d-1,0,d-1)) * invSigma_x[k];\r
1109                             rBrs = C_Lyapunov*rArs + rArs.Transpose()*C_Lyapunov;\r
1110 \r
1111                             if (i==0)\r
1112                                 term = rBrs(0,0);\r
1113                             else\r
1114                                 term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1115 \r
1116                             dc(k*d+i, counter_sigma + i_c) = i1_tmp*term;\r
1117 \r
1118                             if (Options.SEDS_Ver == 2 && jj>=d && ii<d){\r
1119                                 //derivative with respect to the Lyapunov stuffs\r
1120                                 rArs.Zero(); //in fact it is rCrc, but to avoid defining an extra variable, I used rArs\r
1121                                 rArs(ii,jj-d) = 1;\r
1122                                 rBrs = rArs*A[k]+A[k].Transpose()*rArs;\r
1123 \r
1124                                 if (i==0)\r
1125                                     term = rBrs(0,0);\r
1126                                 else\r
1127                                     term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1128 \r
1129                                 dc(k*d+i,counter_C + ii*d+(jj-d)) = i1_tmp*term;\r
1130                             }\r
1131 \r
1132                             jj++;\r
1133                             i_c++;\r
1134                         }\r
1135                     }\r
1136                 }else{ //mse\r
1137                     for (int ii=0; ii<=i; ii++){\r
1138                         for (int jj=0; jj<=i; jj++){\r
1139                             rArs.Zero();\r
1140                             rArs(jj,ii) = 1;\r
1141                             rBrs = C_Lyapunov*rArs + rArs.Transpose()*C_Lyapunov;\r
1142 \r
1143                             if (i==0)\r
1144                                 term = rBrs(0,0);\r
1145                             else\r
1146                                 term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1147 \r
1148                             dc(k*d+i,counter_A + k*d*d + ii*d + jj) = i1_tmp*term;\r
1149 \r
1150                             if (Options.SEDS_Ver == 2){\r
1151                                 //derivative with respect to the Lyapunov stuffs\r
1152                                 rArs.Zero(); //in fact it is rCrc, but to avoid defining an extra variable, I used rArs\r
1153                                 rArs(ii,jj) = 1;\r
1154                                 rBrs = rArs*A[k]+A[k].Transpose()*rArs;\r
1155 \r
1156                                 if (i==0)\r
1157                                     term = rBrs(0,0);\r
1158                                 else\r
1159                                     term = (B_Inv[i]*rBrs.GetMatrix(0,i,0,i)).Trace()*tmp_detB;\r
1160 \r
1161                                 dc(k*d+i,counter_C + ii*d+jj) = i1_tmp*term;\r
1162                             }\r
1163                         }\r
1164                     }\r
1165                 }\r
1166                 i1_tmp *= -1; //used to alternate sign over iterations\r
1167             }\r
1168         }\r
1169     }else{ //eigenvalue\r
1170         Vector eigVal(d), eigVal_new(d);\r
1171         Matrix M_tmp(d,d),eigVec(d,d);\r
1172 \r
1173         for (int k=0; k<K; k++)//for all states\r
1174         {\r
1175             B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov; //define B\r
1176             B.EigenValuesDecomposition(eigVal,eigVec,100);\r
1177             eigVal.Sort();\r
1178             for (int i=0; i<d;i++){\r
1179                 if (!used_for_penalty || eigVal[i] + Options.eps_margin>0)\r
1180                     c(k*d + i) = eigVal[i] + Options.eps_margin;\r
1181             }\r
1182 \r
1183             if (Options.objective){ //likelihood\r
1184                 int i_c = Options.sigma_x_opt*k*d*(d+1) + k*d*d;\r
1185                 for (int ii=0;ii<ind_max_col;ii++){\r
1186                     int jj = (Options.sigma_x_opt) ? ii:d;\r
1187                     while (jj<2*d){\r
1188                         rSrs = L[k];\r
1189                         rSrs(jj,ii) += Options.delta;\r
1190                         rSrs = rSrs*(rSrs.Transpose());\r
1191                         M_tmp = rSrs.GetMatrix(d,2*d-1,0,d-1)*rSrs.GetMatrix(0,d-1,0,d-1).Inverse();\r
1192                         B = C_Lyapunov*M_tmp + M_tmp.Transpose()*C_Lyapunov;\r
1193                         B.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1194                         eigVal_new.Sort();\r
1195                         eigVal_new -= eigVal;\r
1196                         eigVal_new /= Options.delta;\r
1197 \r
1198                         for (int i=0; i<d;i++)\r
1199                             dc(k*d+i,counter_sigma + i_c) = eigVal_new[i];\r
1200 \r
1201 \r
1202                         if (Options.SEDS_Ver == 2 && jj>=d && ii<d){\r
1203                             //derivative with respect to the Lyapunov stuffs\r
1204                             M_tmp = C_Lyapunov;\r
1205                             M_tmp(ii,jj-d) += Options.delta;\r
1206                             M_tmp = M_tmp*A[k]+A[k].Transpose()*M_tmp;\r
1207                             M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1208                             eigVal_new.Sort();\r
1209                             eigVal_new -= eigVal;\r
1210                             eigVal_new /= Options.delta;\r
1211 \r
1212                             for (int i=0; i<d;i++)\r
1213                                 dc(k*d+i,counter_C + ii*d+(jj-d)) = eigVal_new[i];\r
1214                         }\r
1215 \r
1216                         jj++;\r
1217                         i_c++;\r
1218                     }\r
1219                 }\r
1220             }else{ //mse\r
1221                 for (int ii=0;ii<d;ii++){\r
1222                     for (int jj=0;jj<d;jj++){\r
1223                         M_tmp = A[k];\r
1224                         M_tmp(jj,ii) += Options.delta;\r
1225                         M_tmp = C_Lyapunov*M_tmp+M_tmp.Transpose()*C_Lyapunov;\r
1226                         M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1227                         eigVal_new.Sort();\r
1228                         eigVal_new -= eigVal;\r
1229                         eigVal_new /= Options.delta;\r
1230 \r
1231                         for (int i=0; i<d;i++)\r
1232                             dc(k*d+i,counter_A) = eigVal_new[i];\r
1233 \r
1234                         counter_A++;\r
1235 \r
1236                         if (Options.SEDS_Ver == 2){\r
1237                             //derivative with respect to the Lyapunov stuffs\r
1238                             M_tmp = C_Lyapunov;\r
1239                             M_tmp(ii,jj) += Options.delta;\r
1240                             M_tmp = M_tmp*A[k]+A[k].Transpose()*M_tmp;\r
1241                             M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1242                             eigVal_new.Sort();\r
1243                             eigVal_new -= eigVal;\r
1244                             eigVal_new /= Options.delta;\r
1245 \r
1246                             for (int i=0; i<d;i++)\r
1247                                 dc(k*d+i,counter_C + ii*d+jj) = eigVal_new[i];\r
1248                         }\r
1249                     }\r
1250                 }\r
1251             }\r
1252         }\r
1253     }\r
1254 \r
1255 \r
1256     //constraint on positive definiteness of C_Lyapunov\r
1257     if (Options.SEDS_Ver == 2){\r
1258         Vector eigVal(d), eigVal_new(d);\r
1259         Matrix M_tmp(d,d),eigVec(d,d);\r
1260 \r
1261         C_Lyapunov.EigenValuesDecomposition(eigVal,eigVec,100);\r
1262         eigVal.Sort();\r
1263         for (int i=0; i<d;i++){\r
1264             if (!used_for_penalty || eigVal[i] + Options.eps_margin>0)\r
1265                 c(K*d + i) = -eigVal[i] + Options.eps_margin;\r
1266         }\r
1267 \r
1268         //derivative of the constraint on positive definiteness of C_Lyapunov\r
1269         for (int ii=0;ii<d;ii++){\r
1270             for (int jj=0;jj<d;jj++){\r
1271                 M_tmp = C_Lyapunov;\r
1272                 M_tmp(jj,ii) += Options.delta;\r
1273                 M_tmp(ii,jj) += Options.delta;\r
1274                 M_tmp.EigenValuesDecomposition(eigVal_new,eigVec,100);\r
1275                 eigVal_new.Sort();\r
1276                 eigVal_new -= eigVal;\r
1277                 eigVal_new /= Options.delta;\r
1278 \r
1279                 for (int i=0; i<d;i++)\r
1280                     dc(K*d+i,counter_C) = -eigVal_new[i];\r
1281 \r
1282                 counter_C++;\r
1283             }\r
1284         }\r
1285     }\r
1286 }\r
1287 \r
1288 /* Computing the stability constraints */\r
1289 void SEDS::Compute_Constraints(Vector &c){\r
1290     int i1_tmp;\r
1291     double tmp_detB;\r
1292     Vector eigVal(d);\r
1293     Matrix eigVec(d,d);\r
1294     c.Zero();\r
1295 \r
1296     //constraints\r
1297     for (int k=0; k<K; k++)//for all states\r
1298     {\r
1299         i1_tmp = 1;\r
1300         B = A[k]+A[k].Transpose(); //define B\r
1301 \r
1302         if (Options.constraintCriterion){ //principal minor\r
1303             //computing the constraints (this part is basicly an exact rewrite of the matlab version)\r
1304             for (int i=0; i<d; i++) //for all dimensions\r
1305             {\r
1306                 B_Inv[i]=B.GetMatrix(0,i,0,i).Inverse(&tmp_detB);//get inverse and determinants of minors\r
1307                 c(k*d+i) = i1_tmp*tmp_detB+pow(Options.tol_mat_bias,(double)(i+1)/d);\r
1308                 i1_tmp *= -1; //used to alternate sign over iterations\r
1309             }\r
1310         }else{ //eigenvalue\r
1311             B.EigenValuesDecomposition(eigVal,eigVec,100);\r
1312             eigVal.Sort();\r
1313             c.SetSubVector(k*d,eigVal);\r
1314         }\r
1315     }\r
1316 }\r
1317 \r
1318 /* Transforming the GMM model into the vector of optimization's parameters.*/\r
1319 bool SEDS::GMM_2_Parameters_Likelihood(Vector &p){ //this is used to transform GMM to the optimization parameters\r
1320     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1321     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1322     int counter_C = counter_sigma + K*d*d + Options.sigma_x_opt*K*d*(d+1);\r
1323 \r
1324     for (int k=0; k<K; k++){\r
1325         if (Options.perior_opt)\r
1326             p[k] = -log(1.0/Priors[k]-1.0); //insert Priors to p\r
1327 \r
1328         if (Options.mu_opt){\r
1329             for(int j=0; j<d; j++) //for all dimensions\r
1330                 p(counter_mu+k*d+j)=Mu(j,k); //insert Mu\r
1331         }else{\r
1332             Mu_x[k] = Mu.GetColumn(k).GetSubVector(0,d);\r
1333         }\r
1334 \r
1335         Sigma[k].Cholesky(L[k]);//set L\r
1336 \r
1337         if (Options.sigma_x_opt){\r
1338             for(int j=0; j<2*d; j++){\r
1339                 p.InsertSubVector(counter_sigma,L[k].GetColumn(j),j,2*d-j);\r
1340                 counter_sigma += 2*d-j;\r
1341             }\r
1342         }else{\r
1343             for(int j=0; j<d; j++){\r
1344                 p.InsertSubVector(counter_sigma,L[k].GetColumn(j),d,d);\r
1345                 counter_sigma += d;\r
1346             }\r
1347         }\r
1348     }\r
1349 \r
1350     if (Options.SEDS_Ver == 2){\r
1351         Vector tmp;\r
1352         tmp.Set(C_Lyapunov.Array(),d*d);\r
1353         tmp /= 2;\r
1354         p.SetSubVector(counter_C,tmp);\r
1355     }\r
1356     return true;\r
1357 }\r
1358 \r
1359 /* Transforming the vector of optimization's parameters into a GMM model.*/\r
1360 bool SEDS::Parameters_2_GMM_Likelihood(Vector pp){ //this is used to unpack the parameters in p after optimization, to reconstruct the\r
1361     //GMM-parameters in their ususal form: Priors, Mu and Sigma.\r
1362 \r
1363     double sum=0;\r
1364     Vector col(2*d); // a temporary vector needed below\r
1365 \r
1366     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1367     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1368     int counter_C = counter_sigma + K*d*d + Options.sigma_x_opt*K*d*(d+1);\r
1369 \r
1370     for (int k=0; k<K; k=k++){\r
1371         //constructing Priors\r
1372         if (Options.perior_opt){\r
1373             Priors[k] = 1.0/(1.0+exp(-pp[k])); //extract the Priors from correspondng position in optimization vector\r
1374             sum += Priors[k];\r
1375         }\r
1376 \r
1377         //reconstructing Sigma\r
1378         if (Options.sigma_x_opt){\r
1379             for(int j=0; j<2*d; j++){ //for all dimensions\r
1380                 col.Zero();\r
1381                 for(int i=j; i<2*d; i++){\r
1382                     col(i)=pp(counter_sigma);\r
1383                     counter_sigma++;\r
1384                 }\r
1385                 L[k].SetColumn(col, j);\r
1386             }\r
1387         }else{\r
1388             for(int j=0; j<d; j++){ //for all dimensions\r
1389                 for(int i=0; i<d; i++){\r
1390                     L[k](i+d,j) = pp(counter_sigma);\r
1391                     counter_sigma++;\r
1392                 }\r
1393             }\r
1394         }\r
1395 \r
1396         Sigma[k]=L[k]*(L[k].Transpose());\r
1397 \r
1398         for(int i=0; i<2*d;i++)\r
1399             Sigma[k](i,i)+=Options.tol_mat_bias;\r
1400 \r
1401         invSigma[k]=Sigma[k].Inverse(&detSigma(k));\r
1402         invSigma_x[k]=Sigma[k].GetMatrix(0,d-1,0,d-1).Inverse(&detSigma_x[k]);\r
1403         Sigma_xdx[k]=Sigma[k].GetMatrix(d,2*d-1,0,d-1);\r
1404         A[k]=Sigma_xdx[k]*invSigma_x[k]; //dynamical system matrix.\r
1405 \r
1406         if (Options.mu_opt){ //reconstructing Mu\r
1407             Mu_x[k] = pp.GetSubVector(K+k*d,d);\r
1408         }\r
1409         Mu_xd[k] = A[k]*Mu_x[k]; //proagate the centers through the dynamical system\r
1410         for (int i=0; i<d; i++)\r
1411         {\r
1412             if (Options.mu_opt){\r
1413                 Mu(i,k) = Mu_x[k](i);\r
1414             }\r
1415             Mu(i+d,k) = Mu_xd[k](i);\r
1416         }\r
1417     }\r
1418     if (Options.perior_opt)\r
1419         Priors /= sum; //normalizing Priors\r
1420 \r
1421     if (Options.SEDS_Ver == 2){\r
1422         C_Lyapunov.Set(pp.GetSubVector(counter_C,d*d).Array(),d,d);\r
1423         C_Lyapunov = C_Lyapunov + C_Lyapunov.Transpose();\r
1424     }\r
1425 \r
1426     return true;\r
1427 }\r
1428 \r
1429 /* Transforming the GMM model into the vector of optimization's parameters.*/\r
1430 bool SEDS::GMM_2_Parameters_MSE(Vector &p){ //this is used to transform GMM to the optimization parameters\r
1431 \r
1432     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1433     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1434     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1435     int counter_C = counter_A + K*d*d;\r
1436 \r
1437 \r
1438     for (int k=0; k<K; k++){\r
1439         if (Options.perior_opt)\r
1440             p[k] = -log(1.0/Priors[k]-1.0); //insert Priors to p\r
1441 \r
1442         if (Options.mu_opt){\r
1443             for(int j=0; j<d; j++) //for all dimensions\r
1444                 p(counter_mu+k*d+j)=Mu(j,k); //insert Mu\r
1445         }else{\r
1446             Mu_x[k] = Mu.GetColumn(k).GetSubVector(0,d);\r
1447         }\r
1448 \r
1449         Sigma_x[k] = Sigma[k].GetMatrix(0,d-1,0,d-1);\r
1450         invSigma_x[k]=Sigma_x[k].Inverse(&detSigma_x[k]);\r
1451         Sigma_xdx[k] = Sigma[k].GetMatrix(d,2*d-1,0,d-1);\r
1452         A[k] = Sigma_xdx[k]*invSigma_x[k];\r
1453         Sigma_x[k].Cholesky(L_x[k]);//set L_x\r
1454 \r
1455         for(int j=0; j<d; j++){\r
1456             if (Options.sigma_x_opt){\r
1457                 p.InsertSubVector(counter_sigma,L_x[k].GetColumn(j),j,d-j);\r
1458                 counter_sigma += d-j;\r
1459             }\r
1460 \r
1461             p.SetSubVector(counter_A,A[k].GetColumn(j));\r
1462             counter_A += d;\r
1463         }\r
1464     }\r
1465 \r
1466     if (Options.SEDS_Ver == 2){\r
1467         Vector tmp;\r
1468         tmp.Set(C_Lyapunov.Array(),d*d);\r
1469         tmp /= 2;\r
1470         p.SetSubVector(counter_C,tmp);\r
1471     }\r
1472 \r
1473     return true;\r
1474 }\r
1475 \r
1476 /* Transforming the vector of optimization's parameters into a GMM model.*/\r
1477 bool SEDS::Parameters_2_GMM_MSE(Vector pp){ //this is used to unpack the parameters in p after optimization, to reconstruct the\r
1478     //GMM-parameters in their ususal form: Priors, Mu and Sigma.\r
1479 \r
1480     Vector col(d); // a temporary vector needed below\r
1481     int counter_mu = Options.perior_opt*K; //the index at which mu should start\r
1482     int counter_sigma = counter_mu + Options.mu_opt*K*d; //the index at which sigma should start\r
1483     int counter_A = counter_sigma + Options.sigma_x_opt*K*d*(d+1)/2; //the index at which A should start\r
1484     int counter_C = counter_A + K*d*d;\r
1485 \r
1486     for (int k=0; k<K; k=k++){\r
1487         //constructing Priors\r
1488         if (Options.perior_opt)\r
1489             Priors[k] = 1.0/(1.0+exp(-pp[k]));\r
1490 \r
1491         //reconstructing Sigma\r
1492         for(int j=0; j<d; j++){ //for all dimensions\r
1493             col.Zero();\r
1494             for(int i=0; i<d; i++){\r
1495                 if (i>=j && Options.sigma_x_opt){\r
1496                     col(i)=pp(counter_sigma);\r
1497                     counter_sigma++;\r
1498                 }\r
1499                 A[k](i,j) = pp(counter_A);\r
1500                 counter_A++;\r
1501             }\r
1502             if (Options.sigma_x_opt)\r
1503                 L_x[k].SetColumn(col, j);\r
1504         }\r
1505 \r
1506         if (Options.sigma_x_opt){\r
1507             Sigma_x[k]=L_x[k]*(L_x[k].Transpose());\r
1508             for(int i=0; i<d;i++)\r
1509                 Sigma_x[k](i,i)+=Options.tol_mat_bias;\r
1510             invSigma_x[k]=Sigma_x[k].Inverse(&detSigma_x[k]);\r
1511         }\r
1512 \r
1513         Sigma_xdx[k] = A[k]*Sigma_x[k];\r
1514 \r
1515         if (Options.mu_opt)\r
1516             Mu_x[k] = pp.GetSubVector(counter_mu+k*d,d); //reconstructing Mu\r
1517 \r
1518         Mu_xd[k] = A[k]*Mu_x[k]; //proagate the centers through the dynamical system\r
1519         for (int i=0; i<d; i++)\r
1520         {\r
1521             if (Options.mu_opt)\r
1522                 Mu(i,k) = Mu_x[k](i);\r
1523             Mu(i+d,k) = Mu_xd[k](i);\r
1524         }\r
1525     }\r
1526 \r
1527     if (Options.SEDS_Ver == 2){\r
1528         C_Lyapunov.Set(pp.GetSubVector(counter_C,d*d).Array(),d,d);\r
1529         C_Lyapunov = C_Lyapunov + C_Lyapunov.Transpose();\r
1530     }\r
1531     return true;\r
1532 }\r
1533 \r
1534 /* checking if every thing goes well. Sometimes if the parameter\r
1535  * 'Options.cons_penalty' is not big enough, the constrains may be violated.\r
1536  * Then this function notifies the user to increase 'Options.cons_penalty'.\r
1537 */\r
1538 bool SEDS::CheckConstraints(Matrix * A){\r
1539     Vector eigvals;\r
1540     Matrix eigvects;\r
1541     Matrix B(d,d);\r
1542     c.Zero();\r
1543     int nCtrViolated = 0;\r
1544     QString str("");\r
1545 \r
1546     for(int k=0; k<K; k++)\r
1547     {\r
1548         B = C_Lyapunov*A[k]+A[k].Transpose()*C_Lyapunov;\r
1549         B.EigenValuesDecomposition(eigvals,eigvects,100);\r
1550         for(int i=0; i<d; i++){\r
1551             if(eigvals(i) > 0){\r
1552                 if (nCtrViolated == 0)\r
1553                 {\r
1554                     cout << endl;\r
1555                     cout<<"Optimization did not finish successfully. Some constraints were violated."<<endl;\r
1556                     cout<<"The error may be due to change of hard constraints to soft constrints."<<endl;\r
1557                     cout<<"To handle this error, increase the value of 'cons_penalty' and re-run the"<<endl;\r
1558                     cout<<"optimization. Output error for debugging purpose:"<<endl<<endl;\r
1559                     str.sprintf("%s","Optimization did not finish successfully. Some constraints were violated. "\r
1560                                 "The error may be due to change of hard constraints to soft constrints. "\r
1561                                 "To handle this error, increase the value of 'Constraint Penalty' and re-run the "\r
1562                                 "optimization.\nOutput error for debugging purpose:");\r
1563                 }\r
1564                 cout<< "k = " << k << "  ;  err = " << eigvals(i) << endl;\r
1565                 nCtrViolated++;\r
1566                 str.sprintf("%s %.3f",str.toStdString().c_str(),eigvals(i));\r
1567             }\r
1568         }\r
1569     }\r
1570 \r
1571     C_Lyapunov.Print();\r
1572     C_Lyapunov.EigenValuesDecomposition(eigvals,eigvects,100);\r
1573     for(int i=0; i<d; i++){\r
1574         if(eigvals(i) < 0){\r
1575             if (nCtrViolated == 0)\r
1576             {\r
1577                 cout << endl;\r
1578                 cout<<"Optimization did not finish successfully. Some constraints were violated."<<endl;\r
1579                 cout<<"The error may be due to change of hard constraints to soft constrints."<<endl;\r
1580                 cout<<"To handle this error, increase the value of 'cons_penalty' and re-run the"<<endl;\r
1581                 cout<<"optimization. Output error for debugging purpose:"<<endl<<endl;\r
1582                 str.sprintf("%s","Optimization did not finish successfully. Some constraints were violated. "\r
1583                             "The error may be due to change of hard constraints to soft constrints. "\r
1584                             "To handle this error, increase the value of 'Constraint Penalty' and re-run the "\r
1585                             "optimization.\nOutput error for debugging purpose:");\r
1586             }\r
1587             cout<< "C_Lyapunov ;  err = " << eigvals(i) << endl;\r
1588             nCtrViolated++;\r
1589             str.sprintf("%s %.3f",str.toStdString().c_str(),eigvals(i));\r
1590         }\r
1591     }\r
1592 \r
1593     if (nCtrViolated == 0)\r
1594         cout<<"Optimization finished succesfully!"<<endl;\r
1595     else{\r
1596         QMessageBox *qmes = new QMessageBox ();\r
1597         qmes->addButton("Ok",QMessageBox::AcceptRole);\r
1598         qmes->setWindowTitle("Error!");\r
1599         str.sprintf("%s. In total %d constraints were violated.",str.toStdString().c_str(),nCtrViolated);\r
1600         qmes->setText(str);\r
1601         qmes->show();\r
1602     }\r
1603 \r
1604     return true;\r
1605 }\r