Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
UKF_Wind_Estimator.c
Go to the documentation of this file.
1 /*
2  * File: UKF_Wind_Estimator.c
3  *
4  * Code generated for Simulink model 'UKF_Wind_Estimator'.
5  *
6  * Model version : 1.120
7  * Simulink Coder version : 8.10 (R2016a) 10-Feb-2016
8  * C/C++ source code generated on : Wed Nov 2 23:49:42 2016
9  *
10  * Target selection: ert.tlc
11  * Embedded hardware selection: Custom Processor->Custom
12  * Code generation objectives:
13  * 1. Execution efficiency
14  * 2. RAM efficiency
15  * Validation result: Not run
16  */
17 
18 #include "UKF_Wind_Estimator.h"
19 
20 /* Exported data definition */
21 
22 /* Definition for custom storage class: Struct */
25 
26 /* Block signals and states (auto storage) */
28 
29 /* External inputs (root inport signals with auto storage) */
31 
32 /* External outputs (root outports fed by signals with auto storage) */
34 
35 /* Forward declaration for local functions */
36 static real32_T xnrm2_f(int32_T n, const real32_T x[147], int32_T ix0);
37 static void xgeqrf_f(real32_T A[147], real32_T tau[7]);
38 static void qr_e(const real32_T A[147], real32_T Q[147], real32_T R[49]);
39 static real32_T xnrm2(int32_T n, const real32_T x[120], int32_T ix0);
40 static void xgeqrf(real32_T A[120], real32_T tau[6]);
41 static void qr(const real32_T A[120], real32_T Q[120], real32_T R[36]);
42 static void mrdivide(real32_T A[42], const real32_T B_0[36]);
43 static void h(const real32_T x[7], const real32_T q[4], real32_T y[6]);
44 static void RungeKutta(const real32_T x[7], real32_T dt, const real32_T u[6],
45  real32_T xi[7]);
46 
47 /* Function for MATLAB Function: '<Root>/UKF_prediction' */
48 static real32_T xnrm2_f(int32_T n, const real32_T x[147], int32_T ix0)
49 {
50  real32_T y;
52  int32_T kend;
53  real32_T absxk;
54  real32_T t;
55  int32_T k;
56  y = 0.0F;
57  if (!(n < 1)) {
58  if (n == 1) {
59  y = fabsf(x[ix0 - 1]);
60  } else {
61  scale = 1.17549435E-38F;
62  kend = (ix0 + n) - 1;
63  for (k = ix0; k <= kend; k++) {
64  absxk = fabsf(x[k - 1]);
65  if (absxk > scale) {
66  t = scale / absxk;
67  y = y * t * t + 1.0F;
68  scale = absxk;
69  } else {
70  t = absxk / scale;
71  y += t * t;
72  }
73  }
74 
75  y = scale * sqrtf(y);
76  }
77  }
78 
79  return y;
80 }
81 
82 /* Function for MATLAB Function: '<Root>/UKF_prediction' */
83 static void xgeqrf_f(real32_T A[147], real32_T tau[7])
84 {
85  real32_T work[7];
86  int32_T i_i;
87  real32_T b_atmp;
88  real32_T xnorm;
89  int32_T knt;
90  int32_T lastv;
91  int32_T lastc;
92  int32_T ix;
93  real32_T b_c;
94  int32_T iy;
95  int32_T f;
96  int32_T g;
97  int32_T b_ia;
98  int32_T b_ix;
99  boolean_T exitg2;
100  int32_T i;
101  for (i = 0; i < 7; i++) {
102  work[i] = 0.0F;
103  }
104 
105  for (i = 0; i < 7; i++) {
106  i_i = i * 21 + i;
107  b_atmp = A[i_i];
108  b_c = 0.0F;
109  xnorm = xnrm2_f(20 - i, A, i_i + 2);
110  if (xnorm != 0.0F) {
111  xnorm = hypotf(A[i_i], xnorm);
112  if (A[i_i] >= 0.0F) {
113  xnorm = -xnorm;
114  }
115 
116  if (fabsf(xnorm) < 9.86076132E-32F) {
117  knt = 0;
118  do {
119  knt++;
120  lastv = (i_i - i) + 21;
121  for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) {
122  A[lastc] *= 1.01412048E+31F;
123  }
124 
125  xnorm *= 1.01412048E+31F;
126  b_atmp *= 1.01412048E+31F;
127  } while (!(fabsf(xnorm) >= 9.86076132E-32F));
128 
129  xnorm = hypotf(b_atmp, xnrm2_f(20 - i, A, i_i + 2));
130  if (b_atmp >= 0.0F) {
131  xnorm = -xnorm;
132  }
133 
134  b_c = (xnorm - b_atmp) / xnorm;
135  b_atmp = 1.0F / (b_atmp - xnorm);
136  lastv = (i_i - i) + 21;
137  for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) {
138  A[lastc] *= b_atmp;
139  }
140 
141  for (lastv = 1; lastv <= knt; lastv++) {
142  xnorm *= 9.86076132E-32F;
143  }
144 
145  b_atmp = xnorm;
146  } else {
147  b_c = (xnorm - A[i_i]) / xnorm;
148  b_atmp = 1.0F / (A[i_i] - xnorm);
149  knt = (i_i - i) + 21;
150  for (lastv = i_i + 1; lastv + 1 <= knt; lastv++) {
151  A[lastv] *= b_atmp;
152  }
153 
154  b_atmp = xnorm;
155  }
156  }
157 
158  tau[i] = b_c;
159  A[i_i] = b_atmp;
160  if (i + 1 < 7) {
161  b_atmp = A[i_i];
162  A[i_i] = 1.0F;
163  knt = (i + 1) * 21 + i;
164  if (tau[i] != 0.0F) {
165  lastv = 21 - i;
166  lastc = (i_i - i) + 20;
167  while ((lastv > 0) && (A[lastc] == 0.0F)) {
168  lastv--;
169  lastc--;
170  }
171 
172  lastc = 6 - i;
173  exitg2 = false;
174  while ((!exitg2) && (lastc > 0)) {
175  iy = ((lastc - 1) * 21 + knt) + 1;
176  f = iy;
177  do {
178  b_ix = 0;
179  if (f <= (iy + lastv) - 1) {
180  if (A[f - 1] != 0.0F) {
181  b_ix = 1;
182  } else {
183  f++;
184  }
185  } else {
186  lastc--;
187  b_ix = 2;
188  }
189  } while (b_ix == 0);
190 
191  if (b_ix == 1) {
192  exitg2 = true;
193  }
194  }
195  } else {
196  lastv = 0;
197  lastc = 0;
198  }
199 
200  if (lastv > 0) {
201  if (lastc != 0) {
202  for (iy = 1; iy <= lastc; iy++) {
203  work[iy - 1] = 0.0F;
204  }
205 
206  iy = 0;
207  f = ((lastc - 1) * 21 + knt) + 1;
208  for (b_ix = knt + 1; b_ix <= f; b_ix += 21) {
209  ix = i_i;
210  b_c = 0.0F;
211  g = (b_ix + lastv) - 1;
212  for (b_ia = b_ix; b_ia <= g; b_ia++) {
213  b_c += A[b_ia - 1] * A[ix];
214  ix++;
215  }
216 
217  work[iy] += b_c;
218  iy++;
219  }
220  }
221 
222  if (!(-tau[i] == 0.0F)) {
223  iy = 0;
224  for (f = 1; f <= lastc; f++) {
225  if (work[iy] != 0.0F) {
226  b_c = work[iy] * -tau[i];
227  b_ix = i_i;
228  ix = lastv + knt;
229  for (g = knt; g + 1 <= ix; g++) {
230  A[g] += A[b_ix] * b_c;
231  b_ix++;
232  }
233  }
234 
235  iy++;
236  knt += 21;
237  }
238  }
239  }
240 
241  A[i_i] = b_atmp;
242  }
243  }
244 }
245 
246 /* Function for MATLAB Function: '<Root>/UKF_prediction' */
247 static void qr_e(const real32_T A[147], real32_T Q[147], real32_T R[49])
248 {
249  real32_T tau[7];
250  int32_T c_i;
251  int32_T itau;
252  real32_T work[7];
253  int32_T iaii;
254  int32_T lastv;
255  int32_T lastc;
256  int32_T ix;
257  real32_T c;
258  int32_T iy;
259  int32_T iac;
260  int32_T d;
261  int32_T b_ia;
262  int32_T jy;
263  boolean_T exitg2;
264  memcpy(&Q[0], &A[0], 147U * sizeof(real32_T));
265  xgeqrf_f(Q, tau);
266  for (itau = 0; itau < 7; itau++) {
267  for (c_i = 0; c_i + 1 <= itau + 1; c_i++) {
268  R[c_i + 7 * itau] = Q[21 * itau + c_i];
269  }
270 
271  for (c_i = itau + 1; c_i + 1 < 8; c_i++) {
272  R[c_i + 7 * itau] = 0.0F;
273  }
274 
275  work[itau] = 0.0F;
276  }
277 
278  itau = 6;
279  for (c_i = 6; c_i >= 0; c_i += -1) {
280  iaii = (c_i * 21 + c_i) + 1;
281  if (c_i + 1 < 7) {
282  Q[iaii - 1] = 1.0F;
283  if (tau[itau] != 0.0F) {
284  lastv = 21 - c_i;
285  lastc = (iaii - c_i) + 19;
286  while ((lastv > 0) && (Q[lastc] == 0.0F)) {
287  lastv--;
288  lastc--;
289  }
290 
291  lastc = 6 - c_i;
292  exitg2 = false;
293  while ((!exitg2) && (lastc > 0)) {
294  iy = (lastc - 1) * 21 + iaii;
295  jy = iy;
296  do {
297  iac = 0;
298  if (jy + 21 <= (iy + lastv) + 20) {
299  if (Q[jy + 20] != 0.0F) {
300  iac = 1;
301  } else {
302  jy++;
303  }
304  } else {
305  lastc--;
306  iac = 2;
307  }
308  } while (iac == 0);
309 
310  if (iac == 1) {
311  exitg2 = true;
312  }
313  }
314  } else {
315  lastv = 0;
316  lastc = 0;
317  }
318 
319  if (lastv > 0) {
320  if (lastc != 0) {
321  for (iy = 1; iy <= lastc; iy++) {
322  work[iy - 1] = 0.0F;
323  }
324 
325  iy = 0;
326  jy = ((lastc - 1) * 21 + iaii) + 21;
327  for (iac = iaii + 21; iac <= jy; iac += 21) {
328  ix = iaii;
329  c = 0.0F;
330  d = (iac + lastv) - 1;
331  for (b_ia = iac; b_ia <= d; b_ia++) {
332  c += Q[b_ia - 1] * Q[ix - 1];
333  ix++;
334  }
335 
336  work[iy] += c;
337  iy++;
338  }
339  }
340 
341  if (!(-tau[itau] == 0.0F)) {
342  iy = iaii + 20;
343  jy = 0;
344  for (iac = 1; iac <= lastc; iac++) {
345  if (work[jy] != 0.0F) {
346  c = work[jy] * -tau[itau];
347  ix = iaii;
348  d = lastv + iy;
349  for (b_ia = iy; b_ia + 1 <= d; b_ia++) {
350  Q[b_ia] += Q[ix - 1] * c;
351  ix++;
352  }
353  }
354 
355  jy++;
356  iy += 21;
357  }
358  }
359  }
360  }
361 
362  lastv = (iaii - c_i) + 20;
363  for (lastc = iaii; lastc + 1 <= lastv; lastc++) {
364  Q[lastc] *= -tau[itau];
365  }
366 
367  Q[iaii - 1] = 1.0F - tau[itau];
368  for (lastv = 1; lastv <= c_i; lastv++) {
369  Q[(iaii - lastv) - 1] = 0.0F;
370  }
371 
372  itau--;
373  }
374 }
375 
376 /* Function for MATLAB Function: '<Root>/UKF_correction' */
377 static real32_T xnrm2(int32_T n, const real32_T x[120], int32_T ix0)
378 {
379  real32_T y;
380  real32_T scale;
381  int32_T kend;
382  real32_T absxk;
383  real32_T t;
384  int32_T k;
385  y = 0.0F;
386  if (!(n < 1)) {
387  if (n == 1) {
388  y = fabsf(x[ix0 - 1]);
389  } else {
390  scale = 1.17549435E-38F;
391  kend = (ix0 + n) - 1;
392  for (k = ix0; k <= kend; k++) {
393  absxk = fabsf(x[k - 1]);
394  if (absxk > scale) {
395  t = scale / absxk;
396  y = y * t * t + 1.0F;
397  scale = absxk;
398  } else {
399  t = absxk / scale;
400  y += t * t;
401  }
402  }
403 
404  y = scale * sqrtf(y);
405  }
406  }
407 
408  return y;
409 }
410 
411 /* Function for MATLAB Function: '<Root>/UKF_correction' */
412 static void xgeqrf(real32_T A[120], real32_T tau[6])
413 {
414  real32_T work[6];
415  int32_T i_i;
416  real32_T b_atmp;
417  real32_T xnorm;
418  int32_T knt;
419  int32_T lastv;
420  int32_T lastc;
421  int32_T ix;
422  real32_T b_c;
423  int32_T iy;
424  int32_T f;
425  int32_T g;
426  int32_T b_ia;
427  int32_T b_ix;
428  boolean_T exitg2;
429  int32_T i;
430  for (i = 0; i < 6; i++) {
431  work[i] = 0.0F;
432  }
433 
434  for (i = 0; i < 6; i++) {
435  i_i = i * 20 + i;
436  b_atmp = A[i_i];
437  b_c = 0.0F;
438  xnorm = xnrm2(19 - i, A, i_i + 2);
439  if (xnorm != 0.0F) {
440  xnorm = hypotf(A[i_i], xnorm);
441  if (A[i_i] >= 0.0F) {
442  xnorm = -xnorm;
443  }
444 
445  if (fabsf(xnorm) < 9.86076132E-32F) {
446  knt = 0;
447  do {
448  knt++;
449  lastv = (i_i - i) + 20;
450  for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) {
451  A[lastc] *= 1.01412048E+31F;
452  }
453 
454  xnorm *= 1.01412048E+31F;
455  b_atmp *= 1.01412048E+31F;
456  } while (!(fabsf(xnorm) >= 9.86076132E-32F));
457 
458  xnorm = hypotf(b_atmp, xnrm2(19 - i, A, i_i + 2));
459  if (b_atmp >= 0.0F) {
460  xnorm = -xnorm;
461  }
462 
463  b_c = (xnorm - b_atmp) / xnorm;
464  b_atmp = 1.0F / (b_atmp - xnorm);
465  lastv = (i_i - i) + 20;
466  for (lastc = i_i + 1; lastc + 1 <= lastv; lastc++) {
467  A[lastc] *= b_atmp;
468  }
469 
470  for (lastv = 1; lastv <= knt; lastv++) {
471  xnorm *= 9.86076132E-32F;
472  }
473 
474  b_atmp = xnorm;
475  } else {
476  b_c = (xnorm - A[i_i]) / xnorm;
477  b_atmp = 1.0F / (A[i_i] - xnorm);
478  knt = (i_i - i) + 20;
479  for (lastv = i_i + 1; lastv + 1 <= knt; lastv++) {
480  A[lastv] *= b_atmp;
481  }
482 
483  b_atmp = xnorm;
484  }
485  }
486 
487  tau[i] = b_c;
488  A[i_i] = b_atmp;
489  if (i + 1 < 6) {
490  b_atmp = A[i_i];
491  A[i_i] = 1.0F;
492  knt = (i + 1) * 20 + i;
493  if (tau[i] != 0.0F) {
494  lastv = 20 - i;
495  lastc = (i_i - i) + 19;
496  while ((lastv > 0) && (A[lastc] == 0.0F)) {
497  lastv--;
498  lastc--;
499  }
500 
501  lastc = 5 - i;
502  exitg2 = false;
503  while ((!exitg2) && (lastc > 0)) {
504  iy = ((lastc - 1) * 20 + knt) + 1;
505  f = iy;
506  do {
507  b_ix = 0;
508  if (f <= (iy + lastv) - 1) {
509  if (A[f - 1] != 0.0F) {
510  b_ix = 1;
511  } else {
512  f++;
513  }
514  } else {
515  lastc--;
516  b_ix = 2;
517  }
518  } while (b_ix == 0);
519 
520  if (b_ix == 1) {
521  exitg2 = true;
522  }
523  }
524  } else {
525  lastv = 0;
526  lastc = 0;
527  }
528 
529  if (lastv > 0) {
530  if (lastc != 0) {
531  for (iy = 1; iy <= lastc; iy++) {
532  work[iy - 1] = 0.0F;
533  }
534 
535  iy = 0;
536  f = ((lastc - 1) * 20 + knt) + 1;
537  for (b_ix = knt + 1; b_ix <= f; b_ix += 20) {
538  ix = i_i;
539  b_c = 0.0F;
540  g = (b_ix + lastv) - 1;
541  for (b_ia = b_ix; b_ia <= g; b_ia++) {
542  b_c += A[b_ia - 1] * A[ix];
543  ix++;
544  }
545 
546  work[iy] += b_c;
547  iy++;
548  }
549  }
550 
551  if (!(-tau[i] == 0.0F)) {
552  iy = 0;
553  for (f = 1; f <= lastc; f++) {
554  if (work[iy] != 0.0F) {
555  b_c = work[iy] * -tau[i];
556  b_ix = i_i;
557  ix = lastv + knt;
558  for (g = knt; g + 1 <= ix; g++) {
559  A[g] += A[b_ix] * b_c;
560  b_ix++;
561  }
562  }
563 
564  iy++;
565  knt += 20;
566  }
567  }
568  }
569 
570  A[i_i] = b_atmp;
571  }
572  }
573 }
574 
575 /* Function for MATLAB Function: '<Root>/UKF_correction' */
576 static void qr(const real32_T A[120], real32_T Q[120], real32_T R[36])
577 {
578  real32_T tau[6];
579  int32_T c_i;
580  int32_T itau;
581  real32_T work[6];
582  int32_T iaii;
583  int32_T lastv;
584  int32_T lastc;
585  int32_T ix;
586  real32_T c;
587  int32_T iy;
588  int32_T iac;
589  int32_T d;
590  int32_T b_ia;
591  int32_T jy;
592  boolean_T exitg2;
593  memcpy(&Q[0], &A[0], 120U * sizeof(real32_T));
594  xgeqrf(Q, tau);
595  for (itau = 0; itau < 6; itau++) {
596  for (c_i = 0; c_i + 1 <= itau + 1; c_i++) {
597  R[c_i + 6 * itau] = Q[20 * itau + c_i];
598  }
599 
600  for (c_i = itau + 1; c_i + 1 < 7; c_i++) {
601  R[c_i + 6 * itau] = 0.0F;
602  }
603 
604  work[itau] = 0.0F;
605  }
606 
607  itau = 5;
608  for (c_i = 5; c_i >= 0; c_i += -1) {
609  iaii = (c_i * 20 + c_i) + 1;
610  if (c_i + 1 < 6) {
611  Q[iaii - 1] = 1.0F;
612  if (tau[itau] != 0.0F) {
613  lastv = 20 - c_i;
614  lastc = (iaii - c_i) + 18;
615  while ((lastv > 0) && (Q[lastc] == 0.0F)) {
616  lastv--;
617  lastc--;
618  }
619 
620  lastc = 5 - c_i;
621  exitg2 = false;
622  while ((!exitg2) && (lastc > 0)) {
623  iy = (lastc - 1) * 20 + iaii;
624  jy = iy;
625  do {
626  iac = 0;
627  if (jy + 20 <= (iy + lastv) + 19) {
628  if (Q[jy + 19] != 0.0F) {
629  iac = 1;
630  } else {
631  jy++;
632  }
633  } else {
634  lastc--;
635  iac = 2;
636  }
637  } while (iac == 0);
638 
639  if (iac == 1) {
640  exitg2 = true;
641  }
642  }
643  } else {
644  lastv = 0;
645  lastc = 0;
646  }
647 
648  if (lastv > 0) {
649  if (lastc != 0) {
650  for (iy = 1; iy <= lastc; iy++) {
651  work[iy - 1] = 0.0F;
652  }
653 
654  iy = 0;
655  jy = ((lastc - 1) * 20 + iaii) + 20;
656  for (iac = iaii + 20; iac <= jy; iac += 20) {
657  ix = iaii;
658  c = 0.0F;
659  d = (iac + lastv) - 1;
660  for (b_ia = iac; b_ia <= d; b_ia++) {
661  c += Q[b_ia - 1] * Q[ix - 1];
662  ix++;
663  }
664 
665  work[iy] += c;
666  iy++;
667  }
668  }
669 
670  if (!(-tau[itau] == 0.0F)) {
671  iy = iaii + 19;
672  jy = 0;
673  for (iac = 1; iac <= lastc; iac++) {
674  if (work[jy] != 0.0F) {
675  c = work[jy] * -tau[itau];
676  ix = iaii;
677  d = lastv + iy;
678  for (b_ia = iy; b_ia + 1 <= d; b_ia++) {
679  Q[b_ia] += Q[ix - 1] * c;
680  ix++;
681  }
682  }
683 
684  jy++;
685  iy += 20;
686  }
687  }
688  }
689  }
690 
691  lastv = (iaii - c_i) + 19;
692  for (lastc = iaii; lastc + 1 <= lastv; lastc++) {
693  Q[lastc] *= -tau[itau];
694  }
695 
696  Q[iaii - 1] = 1.0F - tau[itau];
697  for (lastv = 1; lastv <= c_i; lastv++) {
698  Q[(iaii - lastv) - 1] = 0.0F;
699  }
700 
701  itau--;
702  }
703 }
704 
705 /* Function for MATLAB Function: '<Root>/main' */
706 static void mrdivide(real32_T A[42], const real32_T B_0[36])
707 {
708  int32_T jp;
709  real32_T temp;
710  real32_T b_A[36];
711  int8_T ipiv[6];
712  int32_T j;
713  int32_T ix;
714  real32_T s;
715  int32_T c_ix;
716  int32_T d;
717  int32_T ijA;
718  int32_T b_jAcol;
719  int32_T b_kBcol;
720  memcpy(&b_A[0], &B_0[0], 36U * sizeof(real32_T));
721  for (j = 0; j < 6; j++) {
722  ipiv[j] = (int8_T)(1 + j);
723  }
724 
725  for (j = 0; j < 5; j++) {
726  jp = j * 7;
727  b_jAcol = 0;
728  ix = jp;
729  temp = fabsf(b_A[jp]);
730  for (b_kBcol = 2; b_kBcol <= 6 - j; b_kBcol++) {
731  ix++;
732  s = fabsf(b_A[ix]);
733  if (s > temp) {
734  b_jAcol = b_kBcol - 1;
735  temp = s;
736  }
737  }
738 
739  if (b_A[jp + b_jAcol] != 0.0F) {
740  if (b_jAcol != 0) {
741  ipiv[j] = (int8_T)((j + b_jAcol) + 1);
742  ix = j;
743  b_jAcol += j;
744  for (b_kBcol = 0; b_kBcol < 6; b_kBcol++) {
745  temp = b_A[ix];
746  b_A[ix] = b_A[b_jAcol];
747  b_A[b_jAcol] = temp;
748  ix += 6;
749  b_jAcol += 6;
750  }
751  }
752 
753  b_jAcol = (jp - j) + 6;
754  for (ix = jp + 1; ix + 1 <= b_jAcol; ix++) {
755  b_A[ix] /= b_A[jp];
756  }
757  }
758 
759  b_jAcol = jp;
760  ix = jp + 6;
761  for (b_kBcol = 1; b_kBcol <= 5 - j; b_kBcol++) {
762  temp = b_A[ix];
763  if (b_A[ix] != 0.0F) {
764  c_ix = jp + 1;
765  d = (b_jAcol - j) + 12;
766  for (ijA = 7 + b_jAcol; ijA + 1 <= d; ijA++) {
767  b_A[ijA] += b_A[c_ix] * -temp;
768  c_ix++;
769  }
770  }
771 
772  ix += 6;
773  b_jAcol += 6;
774  }
775  }
776 
777  for (j = 0; j < 6; j++) {
778  jp = 7 * j;
779  b_jAcol = 6 * j;
780  for (ix = 1; ix <= j; ix++) {
781  b_kBcol = (ix - 1) * 7;
782  if (b_A[(ix + b_jAcol) - 1] != 0.0F) {
783  for (c_ix = 0; c_ix < 7; c_ix++) {
784  A[c_ix + jp] -= b_A[(ix + b_jAcol) - 1] * A[c_ix + b_kBcol];
785  }
786  }
787  }
788 
789  temp = 1.0F / b_A[j + b_jAcol];
790  for (b_jAcol = 0; b_jAcol < 7; b_jAcol++) {
791  A[b_jAcol + jp] *= temp;
792  }
793  }
794 
795  for (j = 5; j >= 0; j += -1) {
796  jp = 7 * j;
797  b_jAcol = 6 * j - 1;
798  for (ix = j + 2; ix < 7; ix++) {
799  b_kBcol = (ix - 1) * 7;
800  if (b_A[ix + b_jAcol] != 0.0F) {
801  for (c_ix = 0; c_ix < 7; c_ix++) {
802  A[c_ix + jp] -= b_A[ix + b_jAcol] * A[c_ix + b_kBcol];
803  }
804  }
805  }
806  }
807 
808  for (j = 4; j >= 0; j += -1) {
809  if (j + 1 != ipiv[j]) {
810  jp = ipiv[j] - 1;
811  for (b_jAcol = 0; b_jAcol < 7; b_jAcol++) {
812  temp = A[7 * j + b_jAcol];
813  A[b_jAcol + 7 * j] = A[7 * jp + b_jAcol];
814  A[b_jAcol + 7 * jp] = temp;
815  }
816  }
817  }
818 }
819 
820 /* Function for MATLAB Function: '<Root>/UKF_correction' */
821 static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
822 {
823  real32_T Vair;
824  real32_T beta;
825  real32_T scale;
826  real32_T t;
827  real32_T qinter_idx_0;
828  real32_T qinv_idx_1;
829  real32_T qinv_idx_2;
830  real32_T qinv_idx_3;
831  real32_T qinter_idx_2;
832  real32_T qinter_idx_3;
833  real32_T qinter_idx_1;
834 
835  /* -----------------Observation_model------------------- */
836  /* description : */
837  /* */
838  /* Dim of the measurement vector dim(zk) = 10; */
839  /* y=[y1 y2 y3 y4 y5 y6 y7 y8 y9 y10] */
840  /* y=[V1 V2 V3 P1 P2 P3 B1 B2 B3 YH ] */
841  /* */
842  /* Inputs : */
843  /* -States: x [15 x 1] */
844  /* -Measures : zk [10 x 1] */
845  /* Outputs : */
846  /* -outputs model : y [10 x 1] */
847  /* ------------------------------------------------------ */
848  scale = 1.17549435E-38F;
849  beta = fabsf(x[0]);
850  if (beta > 1.17549435E-38F) {
851  Vair = 1.0F;
852  scale = beta;
853  } else {
854  t = beta / 1.17549435E-38F;
855  Vair = t * t;
856  }
857 
858  beta = fabsf(x[1]);
859  if (beta > scale) {
860  t = scale / beta;
861  Vair = Vair * t * t + 1.0F;
862  scale = beta;
863  } else {
864  t = beta / scale;
865  Vair += t * t;
866  }
867 
868  beta = fabsf(x[2]);
869  if (beta > scale) {
870  t = scale / beta;
871  Vair = Vair * t * t + 1.0F;
872  scale = beta;
873  } else {
874  t = beta / scale;
875  Vair += t * t;
876  }
877 
878  Vair = scale * sqrtf(Vair);
879 
880  /* ==> Ground speed|R0 = [u,v,w]|R0 + wind|R0 */
881  /* description : */
882  /* q*D*q-1 */
883  scale = ((q[0] * q[0] + q[1] * q[1]) + q[2] * q[2]) + q[3] * q[3];
884  t = q[0] / scale;
885  qinv_idx_1 = -q[1] / scale;
886  qinv_idx_2 = -q[2] / scale;
887  qinv_idx_3 = -q[3] / scale;
888  qinter_idx_0 = -((q[1] * x[0] + q[2] * x[1]) + q[3] * x[2]);
889  qinter_idx_1 = (q[2] * x[2] - q[3] * x[1]) + x[0] * q[0];
890  qinter_idx_2 = (q[3] * x[0] - q[1] * x[2]) + x[1] * q[0];
891  qinter_idx_3 = (q[1] * x[1] - q[2] * x[0]) + x[2] * q[0];
892 
893  /* ==> Va = scale factor * norm([u,v,w]) */
894  if (Vair > 0.0001) {
895  /* ==> AOA = atan(w/u) */
896  scale = atan2f(x[2], x[0]);
897 
898  /* ==> Sideslip = asin(v/||Vair||) */
899  beta = asinf(x[1] / Vair);
900  } else {
901  scale = 0.0F;
902  beta = 0.0F;
903  }
904 
905  y[0] = (((qinter_idx_2 * qinv_idx_3 - qinter_idx_3 * qinv_idx_2) +
906  qinter_idx_1 * t) + qinv_idx_1 * qinter_idx_0) + x[3];
907  y[1] = (((qinter_idx_3 * qinv_idx_1 - qinter_idx_1 * qinv_idx_3) +
908  qinter_idx_2 * t) + qinv_idx_2 * qinter_idx_0) + x[4];
909  y[2] = (((qinter_idx_1 * qinv_idx_2 - qinter_idx_2 * qinv_idx_1) +
910  qinter_idx_3 * t) + qinv_idx_3 * qinter_idx_0) + x[5];
911  y[3] = x[6] * Vair;
912  y[4] = scale;
913  y[5] = beta;
914 }
915 
916 /* Function for MATLAB Function: '<Root>/UKF_prediction' */
917 static void RungeKutta(const real32_T x[7], real32_T dt, const real32_T u[6],
918  real32_T xi[7])
919 {
920  real32_T k2[7];
921  real32_T k3[7];
922  real32_T k4[7];
923  real32_T b_x[7];
924  real32_T y;
925  int32_T i;
926 
927  /* ----------------Runge Kutta 4----------------- */
928  /* description : */
929  /* dt is an integration step (tk+1-tk) */
930  /* h_2 is define as step/2 */
931  /* */
932  /* Non linear function f(x,u), results : X(:,k)=[Xk+1(0) ... Xk+1(2n+1)] */
933  /* =RungeKutta(X(:,k),dt,omega,a); */
934  /* -------------------Process_model--------------------- */
935  /* description : */
936  /* */
937  /* Dim of the state vector : dim(x) = 6; */
938  /* x=[x1 x2 x3 x4 x5 x6] */
939  /* x=[uk vk wk uw0 vw0 ww0] */
940  /* */
941  /* dx=f(x,u) */
942  /* dx1=f(x1,u) */
943  /* dx2=f(x2,u) */
944  /* ... */
945  /* dx=[dx1; dx2; ...] */
946  /* Inputs : */
947  /* -States: x */
948  /* -Command inputs : omega,a */
949  /* Outputs : */
950  /* -state dot : dx */
951  /* ------------------------------------------------------ */
952  /* dot_uvw, dot_wind_bias, dot_airspeed_scale */
953  for (i = 0; i < 7; i++) {
954  xi[i] = 0.0F;
955  }
956 
957  /* a = single(rep(q, u(4:6))); */
958  /* ==> dot_UVW */
959  xi[0] = (u[2] * x[1] + u[3]) - u[1] * x[2];
960  xi[1] = (u[0] * x[2] + u[4]) - u[2] * x[0];
961  xi[2] = (u[1] * x[0] + u[5]) - u[0] * x[1];
962 
963  /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */
964  /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */
965  /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */
966  /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */
967  /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */
968  /* ==> dot_biais */
969  /* already at zero */
970  /* -------------------Process_model--------------------- */
971  /* description : */
972  /* */
973  /* Dim of the state vector : dim(x) = 6; */
974  /* x=[x1 x2 x3 x4 x5 x6] */
975  /* x=[uk vk wk uw0 vw0 ww0] */
976  /* */
977  /* dx=f(x,u) */
978  /* dx1=f(x1,u) */
979  /* dx2=f(x2,u) */
980  /* ... */
981  /* dx=[dx1; dx2; ...] */
982  /* Inputs : */
983  /* -States: x */
984  /* -Command inputs : omega,a */
985  /* Outputs : */
986  /* -state dot : dx */
987  /* ------------------------------------------------------ */
988  /* dot_uvw, dot_wind_bias, dot_airspeed_scale */
989  for (i = 0; i < 7; i++) {
990  b_x[i] = xi[i] / 2.0F * dt + x[i];
991  k2[i] = 0.0F;
992  }
993 
994  /* a = single(rep(q, u(4:6))); */
995  /* ==> dot_UVW */
996  k2[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2];
997  k2[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0];
998  k2[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1];
999 
1000  /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */
1001  /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */
1002  /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */
1003  /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */
1004  /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */
1005  /* ==> dot_biais */
1006  /* already at zero */
1007  /* -------------------Process_model--------------------- */
1008  /* description : */
1009  /* */
1010  /* Dim of the state vector : dim(x) = 6; */
1011  /* x=[x1 x2 x3 x4 x5 x6] */
1012  /* x=[uk vk wk uw0 vw0 ww0] */
1013  /* */
1014  /* dx=f(x,u) */
1015  /* dx1=f(x1,u) */
1016  /* dx2=f(x2,u) */
1017  /* ... */
1018  /* dx=[dx1; dx2; ...] */
1019  /* Inputs : */
1020  /* -States: x */
1021  /* -Command inputs : omega,a */
1022  /* Outputs : */
1023  /* -state dot : dx */
1024  /* ------------------------------------------------------ */
1025  /* dot_uvw, dot_wind_bias, dot_airspeed_scale */
1026  for (i = 0; i < 7; i++) {
1027  b_x[i] = k2[i] / 2.0F * dt + x[i];
1028  k3[i] = 0.0F;
1029  }
1030 
1031  /* a = single(rep(q, u(4:6))); */
1032  /* ==> dot_UVW */
1033  k3[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2];
1034  k3[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0];
1035  k3[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1];
1036 
1037  /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */
1038  /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */
1039  /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */
1040  /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */
1041  /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */
1042  /* ==> dot_biais */
1043  /* already at zero */
1044  /* -------------------Process_model--------------------- */
1045  /* description : */
1046  /* */
1047  /* Dim of the state vector : dim(x) = 6; */
1048  /* x=[x1 x2 x3 x4 x5 x6] */
1049  /* x=[uk vk wk uw0 vw0 ww0] */
1050  /* */
1051  /* dx=f(x,u) */
1052  /* dx1=f(x1,u) */
1053  /* dx2=f(x2,u) */
1054  /* ... */
1055  /* dx=[dx1; dx2; ...] */
1056  /* Inputs : */
1057  /* -States: x */
1058  /* -Command inputs : omega,a */
1059  /* Outputs : */
1060  /* -state dot : dx */
1061  /* ------------------------------------------------------ */
1062  /* dot_uvw, dot_wind_bias, dot_airspeed_scale */
1063  for (i = 0; i < 7; i++) {
1064  b_x[i] = dt * k3[i] + x[i];
1065  k4[i] = 0.0F;
1066  }
1067 
1068  /* a = single(rep(q, u(4:6))); */
1069  /* ==> dot_UVW */
1070  k4[0] = (u[2] * b_x[1] + u[3]) - u[1] * b_x[2];
1071  k4[1] = (u[0] * b_x[2] + u[4]) - u[2] * b_x[0];
1072  k4[2] = (u[1] * b_x[0] + u[5]) - u[0] * b_x[1];
1073 
1074  /* phi = single(atan2(2*(q(1)*q(2)+q(3)*q(4)), 1-2*(q(2)^2+q(3)^2))); */
1075  /* theta = single(asin(2*(q(1)*q(3)-q(2)*q(4)))); */
1076  /* dx(1)=a(1)-9.81*sin(theta)+ omega(3)* x(2)-omega(2)*x(3); */
1077  /* dx(2)=a(2)+9.81*cos(theta)*sin(phi) + omega(1)* x(3)-omega(3)*x(1); */
1078  /* dx(3)=a(3)+9.81*cos(theta)*cos(phi) + omega(2)* x(1)-omega(1)*x(2); */
1079  /* ==> dot_biais */
1080  /* already at zero */
1081  y = dt / 6.0F;
1082  for (i = 0; i < 7; i++) {
1083  xi[i] = (((k2[i] + k3[i]) * 2.0F + xi[i]) + k4[i]) * y + x[i];
1084  }
1085 }
1086 
1087 /* Model step function */
1089 {
1090  real32_T lambda;
1091  int32_T jmax;
1092  int32_T colj;
1093  int32_T b_j;
1094  boolean_T exitg1;
1095  real32_T Y[49];
1096  real32_T u[6];
1097  real32_T SQ[49];
1098  real32_T p[7];
1099  real32_T unusedU0[147];
1100  int32_T jj;
1101  real32_T ajj;
1102  int32_T ix;
1103  int32_T iy;
1104  int32_T b_ix;
1105  int32_T e;
1106  int32_T ia;
1107  real32_T SR[36];
1108  real32_T unusedU0_0[120];
1109  real32_T K[42];
1110  real32_T U[42];
1111  real32_T d[225];
1112  real32_T rtb_x1[7];
1113  real32_T rtb_Wc[15];
1114  real32_T rtb_Wm[15];
1115  real32_T rtb_P1[49];
1116  real32_T rtb_X[105];
1117  real32_T rtb_Y[105];
1118  real32_T rtb_Z[90];
1119  real32_T rtb_z[6];
1120  int32_T i;
1121  real32_T lambda_0[120];
1122  real32_T lambda_1[147];
1123  int32_T b_iy;
1124  real32_T lambda_2[49];
1125  real32_T SR_0[36];
1126  real32_T lambda_3[36];
1127  real32_T rtb_Z_0[90];
1128  real32_T tmp[6];
1129 
1130  /* MATLAB Function: '<Root>/initialization' incorporates:
1131  * Inport: '<Root>/P0'
1132  * Inport: '<Root>/alpha'
1133  * Inport: '<Root>/beta'
1134  * Inport: '<Root>/ki'
1135  * Inport: '<Root>/x0'
1136  * UnitDelay: '<Root>/ Delay'
1137  * UnitDelay: '<Root>/ Delay1'
1138  */
1139  /* MATLAB Function 'initialization': '<S3>:1' */
1140  /* ----------------Initialisation--------------------- */
1141  /* P=0*eye(11); */
1142  /* x=(linspace(0,0,11))'; */
1143  /* persistent Q */
1144  /* persistent R */
1145  if (!ukf_DW.P_not_empty) {
1146  /* '<S3>:1:11' */
1147  /* '<S3>:1:12' */
1148  memcpy(&SQ[0], &(ukf_init.P0[0]), 49U * sizeof(real32_T));
1149  jmax = 0;
1150  colj = 0;
1151  b_j = 1;
1152  exitg1 = false;
1153  while ((!exitg1) && (b_j < 8)) {
1154  jj = (colj + b_j) - 1;
1155  lambda = 0.0F;
1156  if (!(b_j - 1 < 1)) {
1157  ix = colj;
1158  iy = colj;
1159  for (i = 1; i < b_j; i++) {
1160  lambda += SQ[ix] * SQ[iy];
1161  ix++;
1162  iy++;
1163  }
1164  }
1165 
1166  ajj = SQ[jj] - lambda;
1167  if (ajj > 0.0F) {
1168  ajj = sqrtf(ajj);
1169  SQ[jj] = ajj;
1170  if (b_j < 7) {
1171  if (b_j - 1 != 0) {
1172  b_iy = jj + 7;
1173  ix = ((6 - b_j) * 7 + colj) + 8;
1174  for (iy = colj + 8; iy <= ix; iy += 7) {
1175  b_ix = colj;
1176  lambda = 0.0F;
1177  e = (iy + b_j) - 2;
1178  for (ia = iy; ia <= e; ia++) {
1179  lambda += SQ[ia - 1] * SQ[b_ix];
1180  b_ix++;
1181  }
1182 
1183  SQ[b_iy] += -lambda;
1184  b_iy += 7;
1185  }
1186  }
1187 
1188  lambda = 1.0F / ajj;
1189  ix = ((6 - b_j) * 7 + jj) + 8;
1190  for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) {
1191  SQ[b_iy] *= lambda;
1192  }
1193 
1194  colj += 7;
1195  }
1196 
1197  b_j++;
1198  } else {
1199  SQ[jj] = ajj;
1200  jmax = b_j;
1201  exitg1 = true;
1202  }
1203  }
1204 
1205  if (jmax == 0) {
1206  jmax = 7;
1207  } else {
1208  jmax--;
1209  }
1210 
1211  for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1212  for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1213  SQ[colj + 7 * b_j] = 0.0F;
1214  }
1215  }
1216 
1217  for (jmax = 0; jmax < 7; jmax++) {
1218  for (b_j = 0; b_j < 7; b_j++) {
1219  rtb_P1[b_j + 7 * jmax] = SQ[7 * b_j + jmax];
1220  }
1221  }
1222 
1223  ukf_DW.P_not_empty = true;
1224  } else {
1225  /* '<S3>:1:14' */
1226  memcpy(&rtb_P1[0], &ukf_DW.Delay_DSTATE[0], 49U * sizeof(real32_T));
1227  }
1228 
1229  if (!ukf_DW.x_not_empty) {
1230  /* '<S3>:1:17' */
1231  /* '<S3>:1:18' */
1232  for (i = 0; i < 7; i++) {
1233  rtb_x1[i] = ukf_init.x0[i];
1234  }
1235 
1236  ukf_DW.x_not_empty = true;
1237  } else {
1238  /* '<S3>:1:20' */
1239  for (i = 0; i < 7; i++) {
1240  rtb_x1[i] = ukf_DW.Delay1_DSTATE[i];
1241  }
1242  }
1243 
1244  /* %%%%%%%%%%%%%%%--PROGRAMME PRINCIPAL--%%%%%%%%%%%%%%%% */
1245  /* alpha=0.01; %10^-1=<alpha=<1 : dispersion des sigma points. */
1246  /* Default alpha=0.5; */
1247  /* ki=0; %ki>=0 : garantis que la covariance soit semi-definit positive. */
1248  /* Default k=0; */
1249  /* beta=2; %informations sur les moments d'ordre 3 */
1250  /* Default beta=2 choix optimal quand la distrib est sup. gaussienne. */
1251  /* dt=0.02; %dt pour le Runge-Kutta */
1252  /* --------Calcul du lambda----------------- */
1253  /* '<S3>:1:40' */
1254  /* dimension de l'etat */
1255  /* '<S3>:1:42' */
1256  lambda = ukf_init.alpha * ukf_init.alpha * (7.0F + ukf_init.ki) - 7.0F;
1257 
1258  /* '<S3>:1:43' */
1259  /* ----------------Calcul des ponderations Wm et Wc-------------------- */
1260  /* '<S3>:1:47' */
1261  ajj = 0.5F / (7.0F + lambda);
1262  rtb_Wm[0] = lambda / (7.0F + lambda);
1263  for (i = 0; i < 14; i++) {
1264  rtb_Wm[i + 1] = ajj;
1265  }
1266 
1267  /* '<S3>:1:48' */
1268  for (i = 0; i < 15; i++) {
1269  rtb_Wc[i] = rtb_Wm[i];
1270  }
1271 
1272  /* '<S3>:1:49' */
1273  rtb_Wc[0] = ((1.0F - ukf_init.alpha * ukf_init.alpha) + ukf_init.beta) +
1274  rtb_Wm[0];
1275 
1276  /* '<S3>:1:50' */
1277  /* '<S3>:1:51' */
1278  /* '<S3>:1:52' */
1279  lambda = sqrtf(7.0F + lambda);
1280 
1281  /* MATLAB Function: '<Root>/sigmas' */
1282  /* MATLAB Function 'sigmas': '<S5>:1' */
1283  /* ----------------Sigma-point--------------------------- */
1284  /* sigmas points et concatenation : n*(2n+1) */
1285  /* Inputs : */
1286  /* -States: x */
1287  /* -Covariance on state : P */
1288  /* -Weighting squate-root */
1289  /* -sigmas-point (Wm,Wc) c */
1290  /* Outputs : */
1291  /* -Sigmas-points (state) : X */
1292  /* ---------------------------------------------------- */
1293  /* Covariance with ponderation: A = c * P */
1294  /* '<S5>:1:15' */
1295  for (jmax = 0; jmax < 49; jmax++) {
1296  SQ[jmax] = lambda * rtb_P1[jmax];
1297  }
1298 
1299  /* dimension modification on state */
1300  /* '<S5>:1:18' */
1301  /* concatenation : X=[x Y+A Y-A] */
1302  /* '<S5>:1:21' */
1303  for (jmax = 0; jmax < 7; jmax++) {
1304  for (b_j = 0; b_j < 7; b_j++) {
1305  Y[b_j + 7 * jmax] = rtb_x1[b_j];
1306  }
1307 
1308  rtb_X[jmax] = rtb_x1[jmax];
1309  }
1310 
1311  for (jmax = 0; jmax < 49; jmax++) {
1312  rtb_X[jmax + 7] = Y[jmax] + SQ[jmax];
1313  rtb_X[jmax + 56] = Y[jmax] - SQ[jmax];
1314  }
1315 
1316  /* End of MATLAB Function: '<Root>/sigmas' */
1317 
1318  /* MATLAB Function: '<Root>/UKF_prediction' incorporates:
1319  * Inport: '<Root>/Q'
1320  * Inport: '<Root>/accel'
1321  * Inport: '<Root>/dt'
1322  * Inport: '<Root>/rates'
1323  * MATLAB Function: '<Root>/initialization'
1324  */
1325  /* MATLAB Function 'UKF_prediction': '<S2>:1' */
1326  /* '<S2>:1:38' */
1327  /* '<S2>:1:31' */
1328  /* ----------------Prediction Step------------------------- */
1329  /* Sigma point and model fonction f(x,u) */
1330  /* Inputs : */
1331  /* -Sigmas-points (state) : X */
1332  /* -Weighting sigmas-points : Wm, Wc */
1333  /* -Command input : omega, a */
1334  /* -Dimension vector state: n */
1335  /* -Integration step : dt */
1336  /* -Dimension 2n+1 : L */
1337  /* -Weighting state (Wk) : Q */
1338  /* Outputs : */
1339  /* -Predicted state: y */
1340  /* -Sigmas_state : Y */
1341  /* -Covariance on state : H */
1342  /* ---------------------------------------------------- */
1343  /* ----------------Prediction------------------------- */
1344  /* description : */
1345  /* Output : SigmaX(k+1) et xmoy(k+1) */
1346  /* */
1347  /* input state size */
1348  /* sigma state size */
1349  /* '<S2>:1:26' */
1350  for (i = 0; i < 7; i++) {
1351  rtb_x1[i] = 0.0F;
1352  }
1353 
1354  /* '<S2>:1:27' */
1355  memset(&rtb_Y[0], 0, 105U * sizeof(real32_T));
1356 
1357  /* '<S2>:1:29' */
1358  u[0] = ukf_U.rates[0];
1359  u[3] = ukf_U.accel[0];
1360  u[1] = ukf_U.rates[1];
1361  u[4] = ukf_U.accel[1];
1362  u[2] = ukf_U.rates[2];
1363  u[5] = ukf_U.accel[2];
1364 
1365  /* '<S2>:1:30' */
1366  for (i = 0; i < 15; i++) {
1367  /* '<S2>:1:30' */
1368  /* '<S2>:1:31' */
1369  RungeKutta(&rtb_X[7 * i], ukf_params.dt, u, &rtb_Y[7 * i]);
1370 
1371  /* non linear function */
1372  /* '<S2>:1:32' */
1373  for (jmax = 0; jmax < 7; jmax++) {
1374  rtb_x1[jmax] += rtb_Y[7 * i + jmax] * rtb_Wm[i];
1375  }
1376 
1377  /* predicted vector on the state */
1378  /* y = Y(:,1); */
1379  /* '<S2>:1:30' */
1380  }
1381 
1382  /* '<S2>:1:36' */
1383  for (b_iy = 0; b_iy < 49; b_iy++) {
1384  SQ[b_iy] = sqrtf(ukf_params.Q[b_iy]);
1385  }
1386 
1387  /* square-root of R */
1388  /* '<S2>:1:38' */
1389  lambda = sqrtf(rtb_Wm[1]);
1390 
1391  /* '<S2>:1:40' */
1392  /* 33*11=>[11*22 11*11] */
1393  /* '<S2>:1:42' */
1394  for (jmax = 0; jmax < 7; jmax++) {
1395  for (b_j = 0; b_j < 14; b_j++) {
1396  lambda_1[b_j + 21 * jmax] = (rtb_Y[(1 + b_j) * 7 + jmax] - rtb_x1[jmax]) *
1397  lambda;
1398  }
1399  }
1400 
1401  for (jmax = 0; jmax < 7; jmax++) {
1402  for (b_j = 0; b_j < 7; b_j++) {
1403  lambda_1[(b_j + 21 * jmax) + 14] = SQ[7 * b_j + jmax];
1404  }
1405  }
1406 
1407  qr_e(lambda_1, unusedU0, SQ);
1408 
1409  /* '<S2>:1:44' */
1410  for (jmax = 0; jmax < 7; jmax++) {
1411  p[jmax] = rtb_Y[jmax] - rtb_x1[jmax];
1412  }
1413 
1414  if (rtb_Wc[0] < 0.0F) {
1415  /* '<S2>:1:46' */
1416  /* '<S2>:1:47' */
1417  lambda = powf(-rtb_Wc[0], 0.25F);
1418  ajj = powf(-rtb_Wc[0], 0.25F);
1419  for (jmax = 0; jmax < 7; jmax++) {
1420  for (b_j = 0; b_j < 7; b_j++) {
1421  Y[jmax + 7 * b_j] = 0.0F;
1422  for (colj = 0; colj < 7; colj++) {
1423  Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj];
1424  }
1425 
1426  lambda_2[jmax + 7 * b_j] = lambda * p[jmax] * (ajj * p[b_j]);
1427  }
1428  }
1429 
1430  for (jmax = 0; jmax < 7; jmax++) {
1431  for (b_j = 0; b_j < 7; b_j++) {
1432  rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] - lambda_2[7 * jmax + b_j];
1433  }
1434  }
1435 
1436  b_j = 0;
1437  i = 0;
1438  jmax = 1;
1439  exitg1 = false;
1440  while ((!exitg1) && (jmax < 8)) {
1441  colj = (i + jmax) - 1;
1442  lambda = 0.0F;
1443  if (!(jmax - 1 < 1)) {
1444  jj = i;
1445  b_iy = i;
1446  for (ix = 1; ix < jmax; ix++) {
1447  lambda += rtb_P1[jj] * rtb_P1[b_iy];
1448  jj++;
1449  b_iy++;
1450  }
1451  }
1452 
1453  lambda = rtb_P1[colj] - lambda;
1454  if (lambda > 0.0F) {
1455  lambda = sqrtf(lambda);
1456  rtb_P1[colj] = lambda;
1457  if (jmax < 7) {
1458  if (jmax - 1 != 0) {
1459  jj = colj + 7;
1460  b_iy = ((6 - jmax) * 7 + i) + 8;
1461  for (ix = i + 8; ix <= b_iy; ix += 7) {
1462  iy = i;
1463  ajj = 0.0F;
1464  b_ix = (ix + jmax) - 2;
1465  for (e = ix; e <= b_ix; e++) {
1466  ajj += rtb_P1[e - 1] * rtb_P1[iy];
1467  iy++;
1468  }
1469 
1470  rtb_P1[jj] += -ajj;
1471  jj += 7;
1472  }
1473  }
1474 
1475  lambda = 1.0F / lambda;
1476  jj = ((6 - jmax) * 7 + colj) + 8;
1477  for (colj += 7; colj + 1 <= jj; colj += 7) {
1478  rtb_P1[colj] *= lambda;
1479  }
1480 
1481  i += 7;
1482  }
1483 
1484  jmax++;
1485  } else {
1486  rtb_P1[colj] = lambda;
1487  b_j = jmax;
1488  exitg1 = true;
1489  }
1490  }
1491 
1492  if (b_j == 0) {
1493  i = 7;
1494  } else {
1495  i = b_j - 1;
1496  }
1497 
1498  for (b_j = 0; b_j + 1 <= i; b_j++) {
1499  for (jmax = b_j + 1; jmax + 1 <= i; jmax++) {
1500  rtb_P1[jmax + 7 * b_j] = 0.0F;
1501  }
1502  }
1503  } else {
1504  /* '<S2>:1:49' */
1505  lambda = powf(rtb_Wc[0], 0.25F);
1506  ajj = powf(rtb_Wc[0], 0.25F);
1507  for (jmax = 0; jmax < 7; jmax++) {
1508  for (b_j = 0; b_j < 7; b_j++) {
1509  Y[jmax + 7 * b_j] = 0.0F;
1510  for (colj = 0; colj < 7; colj++) {
1511  Y[jmax + 7 * b_j] += SQ[7 * jmax + colj] * SQ[7 * b_j + colj];
1512  }
1513 
1514  lambda_2[jmax + 7 * b_j] = lambda * p[jmax] * (ajj * p[b_j]);
1515  }
1516  }
1517 
1518  for (jmax = 0; jmax < 7; jmax++) {
1519  for (b_j = 0; b_j < 7; b_j++) {
1520  rtb_P1[b_j + 7 * jmax] = Y[7 * jmax + b_j] + lambda_2[7 * jmax + b_j];
1521  }
1522  }
1523 
1524  jmax = 0;
1525  colj = 0;
1526  b_j = 1;
1527  exitg1 = false;
1528  while ((!exitg1) && (b_j < 8)) {
1529  jj = (colj + b_j) - 1;
1530  lambda = 0.0F;
1531  if (!(b_j - 1 < 1)) {
1532  ix = colj;
1533  iy = colj;
1534  for (b_iy = 1; b_iy < b_j; b_iy++) {
1535  lambda += rtb_P1[ix] * rtb_P1[iy];
1536  ix++;
1537  iy++;
1538  }
1539  }
1540 
1541  ajj = rtb_P1[jj] - lambda;
1542  if (ajj > 0.0F) {
1543  ajj = sqrtf(ajj);
1544  rtb_P1[jj] = ajj;
1545  if (b_j < 7) {
1546  if (b_j - 1 != 0) {
1547  b_iy = jj + 7;
1548  ix = ((6 - b_j) * 7 + colj) + 8;
1549  for (iy = colj + 8; iy <= ix; iy += 7) {
1550  b_ix = colj;
1551  lambda = 0.0F;
1552  e = (iy + b_j) - 2;
1553  for (ia = iy; ia <= e; ia++) {
1554  lambda += rtb_P1[ia - 1] * rtb_P1[b_ix];
1555  b_ix++;
1556  }
1557 
1558  rtb_P1[b_iy] += -lambda;
1559  b_iy += 7;
1560  }
1561  }
1562 
1563  lambda = 1.0F / ajj;
1564  ix = ((6 - b_j) * 7 + jj) + 8;
1565  for (i = jj + 7; i + 1 <= ix; i += 7) {
1566  rtb_P1[i] *= lambda;
1567  }
1568 
1569  colj += 7;
1570  }
1571 
1572  b_j++;
1573  } else {
1574  rtb_P1[jj] = ajj;
1575  jmax = b_j;
1576  exitg1 = true;
1577  }
1578  }
1579 
1580  if (jmax == 0) {
1581  jmax = 7;
1582  } else {
1583  jmax--;
1584  }
1585 
1586  for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1587  for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1588  rtb_P1[colj + 7 * b_j] = 0.0F;
1589  }
1590  }
1591  }
1592 
1593  /* End of MATLAB Function: '<Root>/UKF_prediction' */
1594 
1595  /* MATLAB Function: '<Root>/UKF_correction' incorporates:
1596  * Inport: '<Root>/R'
1597  * Inport: '<Root>/q'
1598  * MATLAB Function: '<Root>/initialization'
1599  */
1600  /* MATLAB Function 'UKF_correction': '<S1>:1' */
1601  /* '<S1>:1:49' */
1602  /* '<S1>:1:41' */
1603  /* '<S1>:1:38' */
1604  /* ----------------Correction Step------------------------- */
1605  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
1606  /* By Condomines Jean_Philippe - 1 may 2015- */
1607  /* jp.condomines@gmail.com */
1608  /* %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% */
1609  /* Sigma point and output fonction h(x_k) */
1610  /* Inputs : */
1611  /* -Sigmas-points (state) : X */
1612  /* -Weighting sigmas-points : Wm, Wc */
1613  /* -Dimension of the measurement vector: n */
1614  /* -Dimension 2n+1 : L */
1615  /* -Weighting measures (Vk+1): R */
1616  /* -Measures : zk */
1617  /* Outputs : */
1618  /* -Sigmas_measures : Z */
1619  /* -Predicted measurement : z */
1620  /* -Covariance on measures : J */
1621  /* ---------------------------------------------------- */
1622  /* measurement input size */
1623  /* input state size */
1624  /* sigma state size */
1625  /* Zeros vector for predicted measures */
1626  /* '<S1>:1:29' */
1627  for (i = 0; i < 6; i++) {
1628  rtb_z[i] = 0.0F;
1629  }
1630 
1631  /* Zeros vector for predicted Sigmas_measures */
1632  /* '<S1>:1:32' */
1633  /* "for" loop for "measures predicted" and "sigmas_measures" predicted */
1634  /* '<S1>:1:35' */
1635  for (i = 0; i < 15; i++) {
1636  /* '<S1>:1:35' */
1637  /* non linear function h(x), results : Z(:,k)=[zk+1(0) ... zk+1(2n+1)] */
1638  /* '<S1>:1:38' */
1639  h(&rtb_Y[7 * i], ukf_U.q, u);
1640  for (jmax = 0; jmax < 6; jmax++) {
1641  rtb_Z[jmax + 6 * i] = u[jmax];
1642  }
1643 
1644  /* Predicted measurement */
1645  /* '<S1>:1:41' */
1646  for (jmax = 0; jmax < 6; jmax++) {
1647  rtb_z[jmax] += rtb_Z[6 * i + jmax] * rtb_Wm[i];
1648  }
1649 
1650  /* z = Z(:,1); */
1651  /* '<S1>:1:35' */
1652  }
1653 
1654  /* Square-root of R */
1655  /* '<S1>:1:46' */
1656  for (b_iy = 0; b_iy < 36; b_iy++) {
1657  SR[b_iy] = sqrtf(ukf_params.R[b_iy]);
1658  }
1659 
1660  /* Subtracting of "predicted Sigma measures" et "predicted measurement" with ponderation "Wc" */
1661  /* '<S1>:1:49' */
1662  lambda = sqrtf(rtb_Wm[1]);
1663 
1664  /* Concatenation with the square root of the matrix "R" */
1665  /* '<S1>:1:52' */
1666  /* factorization qr, result use in "J" and rest in "Temp" */
1667  /* Sz,k+1 = qr{[sqrt(Wc(1)) * [Zk+1(:,2:2n+1)-zk+1] sqrt(R)] */
1668  /* '<S1>:1:56' */
1669  for (jmax = 0; jmax < 6; jmax++) {
1670  for (b_j = 0; b_j < 14; b_j++) {
1671  lambda_0[b_j + 20 * jmax] = (rtb_Z[(1 + b_j) * 6 + jmax] - rtb_z[jmax]) *
1672  lambda;
1673  }
1674  }
1675 
1676  for (jmax = 0; jmax < 6; jmax++) {
1677  for (b_j = 0; b_j < 6; b_j++) {
1678  lambda_0[(b_j + 20 * jmax) + 14] = SR[6 * b_j + jmax];
1679  }
1680  }
1681 
1682  qr(lambda_0, unusedU0_0, SR);
1683 
1684  /* Subtracting the first column "zk+1(0)" with all the measurements zk+1 predicted */
1685  /* '<S1>:1:59' */
1686  for (jmax = 0; jmax < 6; jmax++) {
1687  u[jmax] = rtb_Z[jmax] - rtb_z[jmax];
1688  }
1689 
1690  /* Test on Wc(1) for cholesky factorization : Cholupdate{Sz,k+1,p,Wc(0)} */
1691  if (rtb_Wc[0] < 0.0F) {
1692  /* '<S1>:1:62' */
1693  /* '<S1>:1:63' */
1694  lambda = powf(-rtb_Wc[0], 0.25F);
1695  ajj = powf(-rtb_Wc[0], 0.25F);
1696  for (jmax = 0; jmax < 6; jmax++) {
1697  for (b_j = 0; b_j < 6; b_j++) {
1698  SR_0[jmax + 6 * b_j] = 0.0F;
1699  for (colj = 0; colj < 6; colj++) {
1700  SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj];
1701  }
1702 
1703  lambda_3[jmax + 6 * b_j] = lambda * u[jmax] * (ajj * u[b_j]);
1704  }
1705  }
1706 
1707  for (jmax = 0; jmax < 6; jmax++) {
1708  for (b_j = 0; b_j < 6; b_j++) {
1709  SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] - lambda_3[6 * jmax + b_j];
1710  }
1711  }
1712 
1713  b_j = 0;
1714  i = 0;
1715  jmax = 1;
1716  exitg1 = false;
1717  while ((!exitg1) && (jmax < 7)) {
1718  colj = (i + jmax) - 1;
1719  lambda = 0.0F;
1720  if (!(jmax - 1 < 1)) {
1721  jj = i;
1722  b_iy = i;
1723  for (ix = 1; ix < jmax; ix++) {
1724  lambda += SR[jj] * SR[b_iy];
1725  jj++;
1726  b_iy++;
1727  }
1728  }
1729 
1730  lambda = SR[colj] - lambda;
1731  if (lambda > 0.0F) {
1732  lambda = sqrtf(lambda);
1733  SR[colj] = lambda;
1734  if (jmax < 6) {
1735  if (jmax - 1 != 0) {
1736  jj = colj + 6;
1737  b_iy = ((5 - jmax) * 6 + i) + 7;
1738  for (ix = i + 7; ix <= b_iy; ix += 6) {
1739  iy = i;
1740  ajj = 0.0F;
1741  b_ix = (ix + jmax) - 2;
1742  for (e = ix; e <= b_ix; e++) {
1743  ajj += SR[e - 1] * SR[iy];
1744  iy++;
1745  }
1746 
1747  SR[jj] += -ajj;
1748  jj += 6;
1749  }
1750  }
1751 
1752  lambda = 1.0F / lambda;
1753  jj = ((5 - jmax) * 6 + colj) + 7;
1754  for (colj += 6; colj + 1 <= jj; colj += 6) {
1755  SR[colj] *= lambda;
1756  }
1757 
1758  i += 6;
1759  }
1760 
1761  jmax++;
1762  } else {
1763  SR[colj] = lambda;
1764  b_j = jmax;
1765  exitg1 = true;
1766  }
1767  }
1768 
1769  if (b_j == 0) {
1770  i = 6;
1771  } else {
1772  i = b_j - 1;
1773  }
1774 
1775  for (b_j = 0; b_j + 1 <= i; b_j++) {
1776  for (jmax = b_j + 1; jmax + 1 <= i; jmax++) {
1777  SR[jmax + 6 * b_j] = 0.0F;
1778  }
1779  }
1780  } else {
1781  /* '<S1>:1:65' */
1782  lambda = powf(rtb_Wc[0], 0.25F);
1783  ajj = powf(rtb_Wc[0], 0.25F);
1784  for (jmax = 0; jmax < 6; jmax++) {
1785  for (b_j = 0; b_j < 6; b_j++) {
1786  SR_0[jmax + 6 * b_j] = 0.0F;
1787  for (colj = 0; colj < 6; colj++) {
1788  SR_0[jmax + 6 * b_j] += SR[6 * jmax + colj] * SR[6 * b_j + colj];
1789  }
1790 
1791  lambda_3[jmax + 6 * b_j] = lambda * u[jmax] * (ajj * u[b_j]);
1792  }
1793  }
1794 
1795  for (jmax = 0; jmax < 6; jmax++) {
1796  for (b_j = 0; b_j < 6; b_j++) {
1797  SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j] + lambda_3[6 * jmax + b_j];
1798  }
1799  }
1800 
1801  jmax = 0;
1802  colj = 0;
1803  b_j = 1;
1804  exitg1 = false;
1805  while ((!exitg1) && (b_j < 7)) {
1806  jj = (colj + b_j) - 1;
1807  lambda = 0.0F;
1808  if (!(b_j - 1 < 1)) {
1809  ix = colj;
1810  iy = colj;
1811  for (b_iy = 1; b_iy < b_j; b_iy++) {
1812  lambda += SR[ix] * SR[iy];
1813  ix++;
1814  iy++;
1815  }
1816  }
1817 
1818  ajj = SR[jj] - lambda;
1819  if (ajj > 0.0F) {
1820  ajj = sqrtf(ajj);
1821  SR[jj] = ajj;
1822  if (b_j < 6) {
1823  if (b_j - 1 != 0) {
1824  b_iy = jj + 6;
1825  ix = ((5 - b_j) * 6 + colj) + 7;
1826  for (iy = colj + 7; iy <= ix; iy += 6) {
1827  b_ix = colj;
1828  lambda = 0.0F;
1829  e = (iy + b_j) - 2;
1830  for (ia = iy; ia <= e; ia++) {
1831  lambda += SR[ia - 1] * SR[b_ix];
1832  b_ix++;
1833  }
1834 
1835  SR[b_iy] += -lambda;
1836  b_iy += 6;
1837  }
1838  }
1839 
1840  lambda = 1.0F / ajj;
1841  ix = ((5 - b_j) * 6 + jj) + 7;
1842  for (i = jj + 6; i + 1 <= ix; i += 6) {
1843  SR[i] *= lambda;
1844  }
1845 
1846  colj += 6;
1847  }
1848 
1849  b_j++;
1850  } else {
1851  SR[jj] = ajj;
1852  jmax = b_j;
1853  exitg1 = true;
1854  }
1855  }
1856 
1857  if (jmax == 0) {
1858  jmax = 6;
1859  } else {
1860  jmax--;
1861  }
1862 
1863  for (b_j = 0; b_j + 1 <= jmax; b_j++) {
1864  for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
1865  SR[colj + 6 * b_j] = 0.0F;
1866  }
1867  }
1868  }
1869 
1870  /* Transposition to have a lower triangular matrix */
1871  /* '<S1>:1:69' */
1872  for (jmax = 0; jmax < 6; jmax++) {
1873  for (b_j = 0; b_j < 6; b_j++) {
1874  SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax];
1875  }
1876  }
1877 
1878  for (jmax = 0; jmax < 6; jmax++) {
1879  for (b_j = 0; b_j < 6; b_j++) {
1880  SR[b_j + 6 * jmax] = SR_0[6 * jmax + b_j];
1881  }
1882  }
1883 
1884  /* End of MATLAB Function: '<Root>/UKF_correction' */
1885 
1886  /* MATLAB Function: '<Root>/main' incorporates:
1887  * Inport: '<Root>/aoa'
1888  * Inport: '<Root>/sideslip'
1889  * Inport: '<Root>/va '
1890  * Inport: '<Root>/vk'
1891  */
1892  memcpy(&Y[0], &rtb_P1[0], 49U * sizeof(real32_T));
1893 
1894  /* MATLAB Function 'main': '<S4>:1' */
1895  /* '<S4>:1:9' */
1896  /* measurement input size */
1897  /* input state size */
1898  /* sigma state size */
1899  /* '<S4>:1:7' */
1900  /* '<S4>:1:9' */
1901  memset(&d[0], 0, 225U * sizeof(real32_T));
1902 
1903  /* Kalman gain : (Pxz/Sz')/Sz */
1904  /* Caution "/" is least squares root for "Ax = b" */
1905  /* '<S4>:1:13' */
1906  for (b_j = 0; b_j < 15; b_j++) {
1907  d[b_j + 15 * b_j] = rtb_Wc[b_j];
1908  for (jmax = 0; jmax < 7; jmax++) {
1909  rtb_X[jmax + 7 * b_j] = rtb_Y[7 * b_j + jmax] - rtb_x1[jmax];
1910  }
1911  }
1912 
1913  for (jmax = 0; jmax < 7; jmax++) {
1914  for (b_j = 0; b_j < 15; b_j++) {
1915  rtb_Y[jmax + 7 * b_j] = 0.0F;
1916  for (colj = 0; colj < 15; colj++) {
1917  rtb_Y[jmax + 7 * b_j] += rtb_X[7 * colj + jmax] * d[15 * b_j + colj];
1918  }
1919  }
1920  }
1921 
1922  for (jmax = 0; jmax < 6; jmax++) {
1923  for (b_j = 0; b_j < 15; b_j++) {
1924  rtb_Z_0[b_j + 15 * jmax] = rtb_Z[6 * b_j + jmax] - rtb_z[jmax];
1925  }
1926  }
1927 
1928  for (jmax = 0; jmax < 7; jmax++) {
1929  for (b_j = 0; b_j < 6; b_j++) {
1930  K[jmax + 7 * b_j] = 0.0F;
1931  for (colj = 0; colj < 15; colj++) {
1932  K[jmax + 7 * b_j] += rtb_Y[7 * colj + jmax] * rtb_Z_0[15 * b_j + colj];
1933  }
1934  }
1935  }
1936 
1937  for (jmax = 0; jmax < 6; jmax++) {
1938  for (b_j = 0; b_j < 6; b_j++) {
1939  SR_0[b_j + 6 * jmax] = SR[6 * b_j + jmax];
1940  }
1941  }
1942 
1943  mrdivide(K, SR_0);
1944  mrdivide(K, SR);
1945 
1946  /* Predicted state with Kalman gain "K" : x = xpred + K * (zk - z) */
1947  /* '<S4>:1:16' */
1948  /* Quaternion normalisation */
1949  /* x(1:4)=normalQ(x(1:4)'); */
1950  /* Cholesky factorization : U = K * Sz */
1951  /* '<S4>:1:22' */
1952  for (jmax = 0; jmax < 7; jmax++) {
1953  for (b_j = 0; b_j < 6; b_j++) {
1954  U[jmax + 7 * b_j] = 0.0F;
1955  for (colj = 0; colj < 6; colj++) {
1956  U[jmax + 7 * b_j] += K[7 * colj + jmax] * SR[6 * b_j + colj];
1957  }
1958  }
1959  }
1960 
1961  /* "for" loop for cholesky factorization */
1962  /* '<S4>:1:25' */
1963  /* "Sk" in "P" and Transpose of "P" */
1964  /* '<S4>:1:30' */
1965  /* SqrtP = sqrt(diag(Sk*Sk')); */
1966  tmp[0] = ukf_U.vk[0];
1967  tmp[1] = ukf_U.vk[1];
1968  tmp[2] = ukf_U.vk[2];
1969  tmp[3] = ukf_U.va;
1970  tmp[4] = ukf_U.aoa;
1971  tmp[5] = ukf_U.sideslip;
1972  for (i = 0; i < 6; i++) {
1973  /* '<S4>:1:25' */
1974  /* '<S4>:1:26' */
1975  for (jmax = 0; jmax < 7; jmax++) {
1976  for (b_j = 0; b_j < 7; b_j++) {
1977  rtb_P1[jmax + 7 * b_j] = 0.0F;
1978  for (colj = 0; colj < 7; colj++) {
1979  rtb_P1[jmax + 7 * b_j] += Y[7 * jmax + colj] * Y[7 * b_j + colj];
1980  }
1981 
1982  lambda_2[jmax + 7 * b_j] = U[7 * i + jmax] * U[7 * i + b_j];
1983  }
1984  }
1985 
1986  for (jmax = 0; jmax < 7; jmax++) {
1987  for (b_j = 0; b_j < 7; b_j++) {
1988  SQ[b_j + 7 * jmax] = rtb_P1[7 * jmax + b_j] - lambda_2[7 * jmax + b_j];
1989  }
1990  }
1991 
1992  jmax = 0;
1993  colj = 0;
1994  b_j = 1;
1995  exitg1 = false;
1996  while ((!exitg1) && (b_j < 8)) {
1997  jj = (colj + b_j) - 1;
1998  lambda = 0.0F;
1999  if (!(b_j - 1 < 1)) {
2000  ix = colj;
2001  iy = colj;
2002  for (b_iy = 1; b_iy < b_j; b_iy++) {
2003  lambda += SQ[ix] * SQ[iy];
2004  ix++;
2005  iy++;
2006  }
2007  }
2008 
2009  ajj = SQ[jj] - lambda;
2010  if (ajj > 0.0F) {
2011  ajj = sqrtf(ajj);
2012  SQ[jj] = ajj;
2013  if (b_j < 7) {
2014  if (b_j - 1 != 0) {
2015  b_iy = jj + 7;
2016  ix = ((6 - b_j) * 7 + colj) + 8;
2017  for (iy = colj + 8; iy <= ix; iy += 7) {
2018  b_ix = colj;
2019  lambda = 0.0F;
2020  e = (iy + b_j) - 2;
2021  for (ia = iy; ia <= e; ia++) {
2022  lambda += SQ[ia - 1] * SQ[b_ix];
2023  b_ix++;
2024  }
2025 
2026  SQ[b_iy] += -lambda;
2027  b_iy += 7;
2028  }
2029  }
2030 
2031  lambda = 1.0F / ajj;
2032  ix = ((6 - b_j) * 7 + jj) + 8;
2033  for (b_iy = jj + 7; b_iy + 1 <= ix; b_iy += 7) {
2034  SQ[b_iy] *= lambda;
2035  }
2036 
2037  colj += 7;
2038  }
2039 
2040  b_j++;
2041  } else {
2042  SQ[jj] = ajj;
2043  jmax = b_j;
2044  exitg1 = true;
2045  }
2046  }
2047 
2048  if (jmax == 0) {
2049  jmax = 7;
2050  } else {
2051  jmax--;
2052  }
2053 
2054  for (b_j = 0; b_j + 1 <= jmax; b_j++) {
2055  for (colj = b_j + 1; colj + 1 <= jmax; colj++) {
2056  SQ[colj + 7 * b_j] = 0.0F;
2057  }
2058  }
2059 
2060  memcpy(&Y[0], &SQ[0], 49U * sizeof(real32_T));
2061 
2062  /* '<S4>:1:25' */
2063  u[i] = tmp[i] - rtb_z[i];
2064  }
2065 
2066  for (i = 0; i < 7; i++) {
2067  lambda = 0.0F;
2068  for (jmax = 0; jmax < 6; jmax++) {
2069  lambda += K[7 * jmax + i] * u[jmax];
2070  }
2071 
2072  p[i] = rtb_x1[i] + lambda;
2073  for (jmax = 0; jmax < 7; jmax++) {
2074  rtb_P1[jmax + 7 * i] = Y[7 * jmax + i];
2075  }
2076 
2077  /* Outport: '<Root>/xout' */
2078  ukf_Y.xout[i] = p[i];
2079  }
2080 
2081  /* End of MATLAB Function: '<Root>/main' */
2082 
2083  /* Outport: '<Root>/Pout' */
2084  memcpy(&ukf_Y.Pout[0], &rtb_P1[0], 49U * sizeof(real32_T));
2085 
2086  /* Update for UnitDelay: '<Root>/ Delay1' */
2087  for (i = 0; i < 7; i++) {
2088  ukf_DW.Delay1_DSTATE[i] = p[i];
2089  }
2090 
2091  /* End of Update for UnitDelay: '<Root>/ Delay1' */
2092 
2093  /* Update for UnitDelay: '<Root>/ Delay' */
2094  memcpy(&ukf_DW.Delay_DSTATE[0], &rtb_P1[0], 49U * sizeof(real32_T));
2095 }
2096 
2097 /* Model initialize function */
2099 {
2100  /* (no initialization code required) */
2101 }
2102 
2103 /*
2104  * File trailer for generated code.
2105  *
2106  * [EOF]
2107  */
static void xgeqrf_f(real32_T A[147], real32_T tau[7])
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
ukf_init_type ukf_init
static void mrdivide(real32_T A[42], const real32_T B_0[36])
static void RungeKutta(const real32_T x[7], real32_T dt, const real32_T u[6], real32_T xi[7])
ExtY ukf_Y
static real32_T xnrm2(int32_T n, const real32_T x[120], int32_T ix0)
DW ukf_DW
void UKF_Wind_Estimator_initialize(void)
static void qr_e(const real32_T A[147], real32_T Q[147], real32_T R[49])
static void qr(const real32_T A[120], real32_T Q[120], real32_T R[36])
void UKF_Wind_Estimator_step(void)
static real32_T xnrm2_f(int32_T n, const real32_T x[147], int32_T ix0)
ExtU ukf_U
ukf_params_type ukf_params
static void xgeqrf(real32_T A[120], real32_T tau[6])
real32_T aoa
real32_T accel[3]
real32_T Pout[49]
real32_T Delay_DSTATE[49]
real32_T sideslip
real32_T xout[7]
real32_T q[4]
real32_T Delay1_DSTATE[7]
boolean_T x_not_empty
real32_T va
real32_T P0[49]
real32_T rates[3]
real32_T vk[3]
real32_T x0[7]
boolean_T P_not_empty
float g
static const float scale[]
#define A
static float p[2][2]
static uint32_t s
float lambda
unsigned char boolean_T
Definition: rtwtypes.h:64
float real32_T
Definition: rtwtypes.h:55
int int32_T
Definition: rtwtypes.h:53
signed char int8_T
Definition: rtwtypes.h:49
static float K[9]
uint16_t f
Camera baseline, in meters (i.e. horizontal distance between the two cameras of the stereo setup)
Definition: wedgebug.c:204