A sparse MPC solver for walking motion generation.
ip_matrix_ecL.cpp
Go to the documentation of this file.
1 
8 /****************************************
9  * INCLUDES
10  ****************************************/
11 
12 #include "ip_matrix_ecL.h"
13 
14 #include <cmath> // sqrt
15 
16 
17 /****************************************
18  * FUNCTIONS
19  ****************************************/
20 namespace IP
21 {
22  //==============================================
23  // constructors / destructors
24 
25 
26  matrix_ecL::matrix_ecL (const int N)
27  {
28  ecL = new double[MATRIX_SIZE_6x6*N + MATRIX_SIZE_6x6*(N-1)]();
29  }
30 
31 
32  matrix_ecL::~matrix_ecL()
33  {
34  if (ecL != NULL)
35  delete ecL;
36  }
37 
38  //==============================================
39 
40 
41 
54  void matrix_ecL::form_M (
55  const double sinA,
56  const double cosA,
57  const double *i2Q,
58  const double* i2hess)
59  {
60  /* R * Q * R' = M
61  * |c -s | |a1 | |c s | |a1cc+a2ss a1cs-a2cs |
62  * | 1 | | b | | 1 | | b |
63  * | 1 | | g | | 1 | | g |
64  * |s c | | a2 | |-s c | |a1cs-a2cs a1ss+a2cc |
65  * | 1 | | b | | 1 | | b |
66  * | 1| | g| | 1| | g|
67  */
68  // diagonal elements
69  M[0] = i2hess[0]*cosA*cosA + i2hess[1]*sinA*sinA;
70  /*M[28] =*/ M[7] = i2Q[1];
71  /*M[35] =*/ M[14] = i2Q[2];
72  M[21] = i2hess[0]*sinA*sinA + i2hess[1]*cosA*cosA;
73 
74  // symmetric nondiagonal elements
75  /*M[18] =*/ M[3] = (i2hess[0] - i2hess[1])*cosA*sinA;
76  }
77 
78 
79 
89  void matrix_ecL::chol_dec (double *mx)
90  {
91  mx[0] = sqrt(mx[0]);
92  mx[1] /= mx[0];
93  mx[2] /= mx[0];
94  mx[3] /= mx[0];
95  mx[4] /= mx[0];
96  mx[5] /= mx[0];
97 
98  mx[7] = sqrt(mx[7] - mx[1]*mx[1]);
99  mx[8] = (mx[8] - mx[1]*mx[2])/mx[7];
100  mx[9] = (mx[9] - mx[1]*mx[3])/mx[7];
101  mx[10] = (mx[10] - mx[1]*mx[4])/mx[7];
102  mx[11] = (mx[11] - mx[1]*mx[5])/mx[7];
103 
104  mx[14] = sqrt(mx[14] - mx[2]*mx[2] - mx[8]*mx[8]);
105  mx[15] = (mx[15] - mx[2]*mx[3] - mx[8]*mx[9])/mx[14];
106  mx[16] = (mx[16] - mx[2]*mx[4] - mx[8]*mx[10])/mx[14];
107  mx[17] = (mx[17] - mx[2]*mx[5] - mx[8]*mx[11])/mx[14];
108 
109  mx[21] = sqrt(mx[21] - mx[3]*mx[3] - mx[9]*mx[9] - mx[15]*mx[15]);
110  mx[22] = (mx[22] - mx[3]*mx[4] - mx[9]*mx[10] - mx[15]*mx[16])/mx[21];
111  mx[23] = (mx[23] - mx[3]*mx[5] - mx[9]*mx[11] - mx[15]*mx[17])/mx[21];
112 
113  mx[28] = sqrt(mx[28] - mx[4]*mx[4] - mx[10]*mx[10] - mx[16]*mx[16] - mx[22]*mx[22]);
114  mx[29] = (mx[29] - mx[4]*mx[5] - mx[10]*mx[11] - mx[16]*mx[17] - mx[22]*mx[23])/mx[28];
115 
116  mx[35] = sqrt(mx[35] - mx[5]*mx[5] - mx[11]*mx[11] - mx[17]*mx[17] - mx[23]*mx[23] - mx[29]*mx[29]);
117  }
118 
119 
120 
127  void matrix_ecL::form_MAT (const double A3, const double A6)
128  {
129  MAT[0] = M[0];
130  MAT[22] = MAT[1] = A3 * M[7];
131  MAT[23] = MAT[2] = A6 * M[14];
132  MAT[3] = M[3];
133 
134  MAT[28] = MAT[7] = M[7];
135  MAT[29] = MAT[8] = A3 * M[14];
136 
137  MAT[35] = MAT[14] = M[14];
138 
139  MAT[21] = M[21];
140  }
141 
142 
143 
151  void matrix_ecL::form_L_non_diag(const double *ecLp, double *ecLc)
152  {
153  /*
154  * L(k,k) * L(k+1,k)' = -M*A'
155  *
156  * x x x x x
157  * xx xx x xx
158  * xxx * xxxx = xxx
159  * xxxx xxxx x x
160  * xxxxx xxxxx xx
161  * xxxxxx xxxxxx xxx
162  */
163 
164  ecLc[0] = -MAT[0]/ecLp[0];
165  ecLc[3] = -MAT[3]/ecLp[0]; // MAT[3] = MAT[18]
166 
167  ecLc[6] = (-MAT[1] - ecLc[0] * ecLp[1]) / ecLp[7];
168  ecLc[7] = -MAT[7] / ecLp[7];
169  ecLc[9] = (/*0*/ - ecLc[3] * ecLp[1]) / ecLp[7];
170 
171  ecLc[12] = (-MAT[2] - ecLc[0] * ecLp[2] - ecLc[6] * ecLp[8]) / ecLp[14];
172  ecLc[13] = (-MAT[8] - ecLc[7] * ecLp[8]) / ecLp[14];
173  ecLc[14] = -MAT[14] / ecLp[14];
174  ecLc[15] = (/*0*/ - ecLc[3] * ecLp[2] - ecLc[9] * ecLp[8]) / ecLp[14];
175 
176  ecLc[18] = (-MAT[3] - ecLc[0]*ecLp[3] - ecLc[6]*ecLp[9] - ecLc[12]*ecLp[15]) / ecLp[21];
177  ecLc[19] = (/*0*/ - ecLc[7]*ecLp[9] - ecLc[13]*ecLp[15])/ecLp[21];
178  ecLc[20] = (/*0*/ - ecLc[14]*ecLp[15]) / ecLp[21];
179  ecLc[21] = (-MAT[21] - ecLc[3]*ecLp[3] - ecLc[9]*ecLp[9] - ecLc[15]*ecLp[15]) / ecLp[21];
180 
181  ecLc[24] = (/*0*/ - ecLc[0]*ecLp[4] - ecLc[6]*ecLp[10] - ecLc[12]*ecLp[16] - ecLc[18]*ecLp[22]) / ecLp[28];
182  ecLc[25] = (/*0*/ - ecLc[7]*ecLp[10] - ecLc[13]*ecLp[16] - ecLc[19]*ecLp[22]) / ecLp[28];
183  ecLc[26] = (/*0*/ - ecLc[14]*ecLp[16] - ecLc[20]*ecLp[22]) / ecLp[28];
184  ecLc[27] = (-MAT[22] - ecLc[3]*ecLp[4] - ecLc[9]*ecLp[10] - ecLc[15]*ecLp[16] - ecLc[21]*ecLp[22]) / ecLp[28];
185  ecLc[28] = -MAT[28] / ecLp[28];
186 
187  ecLc[30] = (/*0*/ - ecLc[0]*ecLp[5] - ecLc[6]*ecLp[11] - ecLc[12]*ecLp[17] - ecLc[18]*ecLp[23] - ecLc[24]*ecLp[29]) / ecLp[35];
188  ecLc[31] = (/*0*/ - ecLc[7]*ecLp[11] - ecLc[13]*ecLp[17] - ecLc[19]*ecLp[23] - ecLc[25]*ecLp[29]) / ecLp[35];
189  ecLc[32] = (/*0*/ - ecLc[14]*ecLp[17] - ecLc[20]*ecLp[23] - ecLc[26]*ecLp[29]) / ecLp[35];
190  ecLc[33] = (-MAT[23] - ecLc[3]*ecLp[5] - ecLc[9]*ecLp[11] - ecLc[15]*ecLp[17] - ecLc[21]*ecLp[23] - ecLc[27]*ecLp[29]) / ecLp[35];
191  ecLc[34] = (-MAT[29] - ecLc[28]*ecLp[29])/ ecLp[35];
192  ecLc[35] = -MAT[35] / ecLp[35];
193 
194 
195  // ecLc[1] = ecLc[2] = ecLc[4] = ecLc[5] = ecLc[8] = ecLc[10] =
196  // = ecLc[11] = ecLc[16] = ecLc[17] = ecLc[22] = ecLc[23] = ecLc[29] = 0
197  }
198 
199 
209  void matrix_ecL::form_L_diag(const double *B, const double i2P, double *ecLc)
210  {
211  // diagonal elements
212  ecLc[0] = i2P * B[0]*B[0] + M[0];
213  ecLc[28] = ecLc[7] = i2P * B[1]*B[1] + M[7];
214  ecLc[35] = ecLc[14] = i2P * B[2]*B[2] + M[14];
215  ecLc[21] = i2P * B[0]*B[0] + M[21];
216 
217  // symmetric elements (no need to initialize all of them)
218  ecLc[22] = ecLc[1] = i2P * B[0]*B[1];
219  ecLc[23] = ecLc[2] = i2P * B[0]*B[2];
220  ecLc[29] = ecLc[8] = i2P * B[1]*B[2];
221  ecLc[3] = M[3];
222 
223  // reset elements
224  ecLc[4] = ecLc[5] = ecLc[9] = ecLc[10] = ecLc[11] =
225  ecLc[15] = ecLc[16] = ecLc[17] = 0;
226 
227 
228  // chol (L(k+1,k+1))
229  chol_dec (ecLc);
230  }
231 
232 
233 
246  void matrix_ecL::form_AMATMBiPB(const double A3, const double A6, const double *B, const double i2P, double *result)
247  {
248  const double tmpvar = A3*MAT[1] + A6*MAT[2] + i2P * B[0]*B[0];
249 
250  result[0] = tmpvar + M[0] + MAT[0];
251  result[22] = result[1] = i2P * B[0]*B[1] + MAT[1] + A3*MAT[2];
252  result[23] = result[2] = i2P * B[0]*B[2] + MAT[2];
253  result[3] = M[3] + MAT[3];
254 
255  result[28] = result[7] = i2P * B[1]*B[1] + M[7] + MAT[7] + A3*MAT[8];
256  result[29] = result[8] = i2P * B[1]*B[2] + MAT[8];
257 
258  result[35] = result[14] = i2P * B[2]*B[2] + M[14] + MAT[14];
259 
260  result[21] = tmpvar + M[21] + MAT[21];
261  }
262 
263 
264 
275  void matrix_ecL::form_L_diag (const double *p, double *ecLc)
276  {
277  /* - L(k+1,k) * L(k+1,k)' + A*M*A' + MBiPB
278  * xxxxxx x x
279  * xxxxx xx x
280  * xxxx xxxx
281  * xxxxxx * xxxx
282  * xx xxxxx
283  * x xxxxxx
284  */
285 
286  ecLc[0] += - p[0] *p[0] - p[6] *p[6] - p[12]*p[12] - p[18]*p[18] - p[24]*p[24] - p[30]*p[30];
287  ecLc[1] += - p[6] *p[7] - p[12]*p[13] - p[18]*p[19] - p[24]*p[25] - p[30]*p[31];
288  ecLc[2] += - p[12]*p[14] - p[18]*p[20] - p[24]*p[26] - p[30]*p[32];
289  ecLc[3] += - p[0] *p[3] - p[6] *p[9] - p[12]*p[15] - p[18]*p[21] - p[24]*p[27] - p[30]*p[33];
290  ecLc[4] = - p[24]*p[28] - p[30]*p[34];
291  ecLc[5] = - p[30]*p[35];
292 
293  ecLc[7] += - p[7] *p[7] - p[13]*p[13] - p[19]*p[19] - p[25]*p[25] - p[31]*p[31];
294  ecLc[8] += - p[13]*p[14] - p[19]*p[20] - p[25]*p[26] - p[31]*p[32];
295  ecLc[9] = - p[7] *p[9] - p[13]*p[15] - p[19]*p[21] - p[25]*p[27] - p[31]*p[33];
296  ecLc[10] = - p[25]*p[28] - p[31]*p[34];
297  ecLc[11] = - p[31]*p[35];
298 
299  ecLc[14] += - p[14]*p[14] - p[20]*p[20] - p[26]*p[26] - p[32]*p[32];
300  ecLc[15] = - p[14]*p[15] - p[20]*p[21] - p[26]*p[27] - p[32]*p[33];
301  ecLc[16] = - p[26]*p[28] - p[32]*p[34];
302  ecLc[17] = - p[32]*p[35];
303 
304  ecLc[21] += - p[3] *p[3] - p[9] *p[9] - p[15]*p[15] - p[21]*p[21] - p[27]*p[27] - p[33]*p[33];
305  ecLc[22] += - p[27]*p[28] - p[33]*p[34];
306  ecLc[23] += - p[33]*p[35];
307 
308  ecLc[28] += - p[28]*p[28] - p[34]*p[34];
309  ecLc[29] += - p[34]*p[35];
310 
311  ecLc[35] += - p[35]*p[35];
312 
313 
314  // chol (L(k+1,k+1))
315  chol_dec (ecLc);
316  }
317 
318 
319 
326  void matrix_ecL::form (const problem_parameters& ppar, const double *i2hess)
327  {
328  int i;
329  state_parameters stp;
330 
331  stp = ppar.spar[0];
332 
333  // the first matrix on diagonal
334  form_M (stp.sin, stp.cos, ppar.i2Q, i2hess);
335  form_L_diag (stp.B, ppar.i2P, ecL);
336 
337  // offsets
338  double *ecL_cur = &ecL[MATRIX_SIZE_6x6];
339  double *ecL_prev = &ecL[0];
340  for (i = 1; i < ppar.N; i++)
341  {
342  stp = ppar.spar[i];
343 
344  // form all matrices
345  form_MAT (stp.A3, stp.A6);
346  form_L_non_diag (ecL_prev, ecL_cur);
347 
348  // update offsets
349  ecL_cur = &ecL_cur[MATRIX_SIZE_6x6];
350  ecL_prev = &ecL_prev[MATRIX_SIZE_6x6];
351 
352 
353  i2hess = &i2hess[2];
354  form_M (stp.sin, stp.cos, ppar.i2Q, i2hess);
355  form_AMATMBiPB(stp.A3, stp.A6, stp.B, ppar.i2P, ecL_cur);
356  form_L_diag(ecL_prev, ecL_cur);
357 
358  // update offsets
359  ecL_cur = &ecL_cur[MATRIX_SIZE_6x6];
360  ecL_prev = &ecL_prev[MATRIX_SIZE_6x6];
361  }
362  }
363 
364 
365 
373  void matrix_ecL::solve_forward(const int N, double *x)
374  {
375  double *xc = x; // 6 current elements of x
376  double *xp; // 6 elements of x computed on the previous iteration
377  double *ecL_cur = &ecL[0]; // lower triangular matrix lying on the
378  // diagonal of L
379  double *ecL_prev; // upper triangular matrix lying to the left from
380  // ecL_cur at the same level of L
381 
382 
383  // compute the first 6 elements using forward substitution
384  xc[0] /= ecL_cur[0];
385  xc[1] = (xc[1] - xc[0]*ecL_cur[1]) / ecL_cur[7];
386  xc[2] = (xc[2] - xc[0]*ecL_cur[2] - xc[1]*ecL_cur[8]) / ecL_cur[14];
387  xc[3] = (xc[3] - xc[0]*ecL_cur[3] - xc[1]*ecL_cur[9] - xc[2]*ecL_cur[15]) / ecL_cur[21];
388  xc[4] = (xc[4] - xc[0]*ecL_cur[4] - xc[1]*ecL_cur[10] - xc[2]*ecL_cur[16] - xc[3]*ecL_cur[22]) / ecL_cur[28];
389  xc[5] = (xc[5] - xc[0]*ecL_cur[5] - xc[1]*ecL_cur[11] - xc[2]*ecL_cur[17] - xc[3]*ecL_cur[23] - xc[4]*ecL_cur[29]) / ecL_cur[35];
390 
391 
392  for (int i = 1; i < N; i++)
393  {
394  // switch to the next level of L / next 6 elements
395  xp = xc;
396  xc = &xc[SMPC_NUM_STATE_VAR];
397 
398  ecL_prev = &ecL_cur[MATRIX_SIZE_6x6];
399  ecL_cur = &ecL_prev[MATRIX_SIZE_6x6];
400 
401 
402  // update the right part of the equation
403  /*
404  * xxxxxx
405  * xxxxx
406  * xxxx
407  * xxxxxx
408  * xx
409  * x
410  */
411  xc[0] -= xp[0]*ecL_prev[0] + xp[1]*ecL_prev[6] + xp[2]*ecL_prev[12] + xp[3]*ecL_prev[18] + xp[4]*ecL_prev[24] + xp[5]*ecL_prev[30];
412  xc[1] -= xp[1]*ecL_prev[7] + xp[2]*ecL_prev[13] + xp[3]*ecL_prev[19] + xp[4]*ecL_prev[25] + xp[5]*ecL_prev[31];
413  xc[2] -= xp[2]*ecL_prev[14] + xp[3]*ecL_prev[20] + xp[4]*ecL_prev[26] + xp[5]*ecL_prev[32];
414  xc[3] -= xp[0]*ecL_prev[3] + xp[1]*ecL_prev[9] + xp[2]*ecL_prev[15] + xp[3]*ecL_prev[21] + xp[4]*ecL_prev[27] + xp[5]*ecL_prev[33];
415  xc[4] -= xp[4]*ecL_prev[28] + xp[5]*ecL_prev[34];
416  xc[5] -= xp[5]*ecL_prev[35];
417 
418  // forward substitution
419  xc[0] /= ecL_cur[0];
420  xc[1] = (xc[1] - xc[0]*ecL_cur[1]) / ecL_cur[7];
421  xc[2] = (xc[2] - xc[0]*ecL_cur[2] - xc[1]*ecL_cur[8]) / ecL_cur[14];
422  xc[3] = (xc[3] - xc[0]*ecL_cur[3] - xc[1]*ecL_cur[9] - xc[2]*ecL_cur[15]) / ecL_cur[21];
423  xc[4] = (xc[4] - xc[0]*ecL_cur[4] - xc[1]*ecL_cur[10] - xc[2]*ecL_cur[16] - xc[3]*ecL_cur[22]) / ecL_cur[28];
424  xc[5] = (xc[5] - xc[0]*ecL_cur[5] - xc[1]*ecL_cur[11] - xc[2]*ecL_cur[17] - xc[3]*ecL_cur[23] - xc[4]*ecL_cur[29]) / ecL_cur[35];
425  }
426  }
427 
428 
435  void matrix_ecL::solve_backward (const int N, double *x)
436  {
437  double *xc = & x[(N-1)*SMPC_NUM_STATE_VAR]; // current 6 elements of result
438  double *xp; // 6 elements computed on the previous iteration
439 
440  // elements of these matrices accessed as if they were transposed
441  // lower triangular matrix lying on the diagonal of L
442  double *ecL_cur = &ecL[2 * (N - 1) * MATRIX_SIZE_6x6];
443  // upper triangular matrix lying to the right from ecL_cur at the same level of L'
444  double *ecL_prev;
445 
446 
447  // compute the last 6 elements using backward substitution
448  xc[5] /= ecL_cur[35];
449  xc[4] = (xc[4] - xc[5]*ecL_cur[29]) / ecL_cur[28];
450  xc[3] = (xc[3] - xc[5]*ecL_cur[23] - xc[4]*ecL_cur[22]) / ecL_cur[21];
451  xc[2] = (xc[2] - xc[5]*ecL_cur[17] - xc[4]*ecL_cur[16] - xc[3]*ecL_cur[15]) / ecL_cur[14];
452  xc[1] = (xc[1] - xc[5]*ecL_cur[11] - xc[4]*ecL_cur[10] - xc[3]*ecL_cur[9] - xc[2]*ecL_cur[8]) / ecL_cur[7];
453  xc[0] = (xc[0] - xc[5]*ecL_cur[5] - xc[4]*ecL_cur[4] - xc[3]*ecL_cur[3] - xc[2]*ecL_cur[2] - xc[1]*ecL_cur[1]) / ecL_cur[0];
454 
455  for (int i = N-2; i >= 0 ; i--)
456  {
457  xp = xc;
458  xc = & x[i*SMPC_NUM_STATE_VAR];
459 
460  ecL_cur = &ecL[2 * i * MATRIX_SIZE_6x6];
461  ecL_prev = &ecL_cur[MATRIX_SIZE_6x6];
462 
463 
464  // update the right part of the equation
465  /*
466  * x x
467  * xx x
468  * xxxx
469  * xxxx
470  * xxxxx
471  * xxxxxx
472  */
473  xc[0] -= xp[0]*ecL_prev[0] + xp[3]*ecL_prev[3];
474  xc[1] -= xp[0]*ecL_prev[6] + xp[1]*ecL_prev[7] + xp[3]*ecL_prev[9];
475  xc[2] -= xp[0]*ecL_prev[12] + xp[1]*ecL_prev[13] + xp[2]*ecL_prev[14] + xp[3]*ecL_prev[15];
476  xc[3] -= xp[0]*ecL_prev[18] + xp[1]*ecL_prev[19] + xp[2]*ecL_prev[20] + xp[3]*ecL_prev[21];
477  xc[4] -= xp[0]*ecL_prev[24] + xp[1]*ecL_prev[25] + xp[2]*ecL_prev[26] + xp[3]*ecL_prev[27] + xp[4]*ecL_prev[28];
478  xc[5] -= xp[0]*ecL_prev[30] + xp[1]*ecL_prev[31] + xp[2]*ecL_prev[32] + xp[3]*ecL_prev[33] + xp[4]*ecL_prev[34] + xp[5]*ecL_prev[35];
479 
480  // backward substitution
481  xc[5] /= ecL_cur[35];
482  xc[4] = (xc[4] - xc[5]*ecL_cur[29]) / ecL_cur[28];
483  xc[3] = (xc[3] - xc[5]*ecL_cur[23] - xc[4]*ecL_cur[22]) / ecL_cur[21];
484  xc[2] = (xc[2] - xc[5]*ecL_cur[17] - xc[4]*ecL_cur[16] - xc[3]*ecL_cur[15]) / ecL_cur[14];
485  xc[1] = (xc[1] - xc[5]*ecL_cur[11] - xc[4]*ecL_cur[10] - xc[3]*ecL_cur[9] - xc[2]*ecL_cur[8]) / ecL_cur[7];
486  xc[0] = (xc[0] - xc[5]*ecL_cur[5] - xc[4]*ecL_cur[4] - xc[3]*ecL_cur[3] - xc[2]*ecL_cur[2] - xc[1]*ecL_cur[1]) / ecL_cur[0];
487  }
488  }
489 }
SMPC_NUM_STATE_VAR
#define SMPC_NUM_STATE_VAR
Number of state variables.
Definition: smpc_solver.h:24
IP::matrix_ecL::ecL
double * ecL
Definition: ip_matrix_ecL.h:54
IP::problem_parameters
A set of problem parameters.
Definition: ip_problem_param.h:48
ip_matrix_ecL.h
IP::matrix_ecL::matrix_ecL
matrix_ecL(const int)
Definition: ip_matrix_ecL.cpp:26
IP::state_parameters::A3
double A3
Definition: ip_problem_param.h:38
IP::matrix_ecL::solve_backward
void solve_backward(const int, double *)
Solve system ecL' * x = b using backward substitution.
Definition: ip_matrix_ecL.cpp:435
IP::matrix_ecL::~matrix_ecL
~matrix_ecL()
Definition: ip_matrix_ecL.cpp:32
IP::state_parameters::A6
double A6
Definition: ip_problem_param.h:39
IP::matrix_ecL::MAT
double MAT[MATRIX_SIZE_6x6]
R * inv(Q) * R'.
Definition: ip_matrix_ecL.h:71
IP::matrix_ecL::M
double M[MATRIX_SIZE_6x6]
Definition: ip_matrix_ecL.h:70
IP::state_parameters::sin
double sin
Definition: ip_problem_param.h:30
IP::state_parameters
Definition: ip_problem_param.h:26
IP::matrix_ecL::form_M
void form_M(const double, const double, const double *, const double *)
Forms M = R*inv(hess_phi)*R'.
Definition: ip_matrix_ecL.cpp:54
IP::problem_parameters::N
int N
Definition: ip_problem_param.h:58
IP::problem_parameters::spar
state_parameters * spar
Definition: ip_problem_param.h:75
IP::state_parameters::cos
double cos
Definition: ip_problem_param.h:29
MATRIX_SIZE_6x6
#define MATRIX_SIZE_6x6
The number of elements in 6x6 matrix.
Definition: ip_matrix_ecL.h:32
IP::problem_parameters::i2Q
double i2Q[3]
Definition: ip_problem_param.h:63
IP::matrix_ecL::chol_dec
void chol_dec(double *)
Performs Cholesky decomposition of a matrix.
Definition: ip_matrix_ecL.cpp:89
IP::state_parameters::B
double B[3]
Definition: ip_problem_param.h:41
IP::problem_parameters::i2P
double i2P
Definition: ip_problem_param.h:68
IP::matrix_ecL::form
void form(const problem_parameters &, const double *)
Builds matrix L.
Definition: ip_matrix_ecL.cpp:326
IP::matrix_ecL::solve_forward
void solve_forward(const int, double *)
Solve system ecL * x = b using forward substitution.
Definition: ip_matrix_ecL.cpp:373
IP::matrix_ecL::form_L_non_diag
void form_L_non_diag(const double *, double *)
Forms a 6x6 matrix L(k+1, k), which lies below the diagonal of L.
Definition: ip_matrix_ecL.cpp:151
IP::matrix_ecL::form_MAT
void form_MAT(const double, const double)
Forms matrix MAT = M * A'.
Definition: ip_matrix_ecL.cpp:127
IP::matrix_ecL::form_L_diag
void form_L_diag(const double *, const double, double *)
Forms a 6x6 matrix L(0, 0) = chol (M + B * inv(2*P) * B).
Definition: ip_matrix_ecL.cpp:209
IP::matrix_ecL::form_AMATMBiPB
void form_AMATMBiPB(const double, const double, const double *, const double, double *)
Forms matrix AMATMBiPB = A * M1 * A' + 0.5 * M2 + 0.5 * B * inv(P) * B.
Definition: ip_matrix_ecL.cpp:246
Generated by   doxygen 1.8.15