37 static void svd_bidiagonal_trans_and_store_diag_elem(uint8_t m, uint8_t n,
39 uint8_t u_m, uint8_t u_n,
46 static void svd_setup_bi_diagonal_matrix(uint8_t m, uint8_t n,
matrix_t A[m][n],
53 static void svd_compute_singular_values_U_V(uint8_t m, uint8_t n, uint8_t u_n,
59 static void svd_generate_S(uint8_t m, uint8_t n,
matrix_t S[n][n],
60 uint8_t sing_vec_length,
matrix_t singl_values_vec[]);
62 static int32_t min(int32_t a, int32_t b)
72 static int32_t max(int32_t a, int32_t b)
87 for (i = 1; i < length; i++) {
88 if (fabs(arr[i]) > max) {
116 return min(m + 1, n);
120 uint8_t u_m, uint8_t u_n,
matrix_t U[u_m][u_n],
122 uint8_t sing_vec_length,
matrix_t singl_values_vec[])
125 int8_t u_n_min = min(m, n);
131 memset(singl_values_vec, 0,
sizeof(
matrix_t) * sing_vec_length);
133 svd_bidiagonal_trans_and_store_diag_elem(m, n, A, u_m, u_n, U, V,
134 sup_diag_elem_vec, singl_values_vec);
136 svd_setup_bi_diagonal_matrix(m, n, A, u_n, U, V, sup_diag_elem_vec,
139 svd_compute_singular_values_U_V(m, n, u_n, U, V, sup_diag_elem_vec,
142 svd_generate_S(u_n, n, S, sing_vec_length, singl_values_vec);
166 static void svd_bidiagonal_trans_and_store_diag_elem(uint8_t m, uint8_t n,
168 uint8_t u_m, uint8_t u_n,
174 int col_trans_num = min(m - 1, n);
175 int row_trans_num = max(0, min(n - 2, m));
176 int upper_lim = max(col_trans_num, row_trans_num);
178 for (
int index = 0; index < upper_lim; index++) {
179 if (index < col_trans_num) {
180 diag_elem_vec[index] = 0;
181 for (
int row_num = index; row_num < m; row_num++) {
182 diag_elem_vec[index] = hypot(
183 diag_elem_vec[index],
186 if (diag_elem_vec[index] != 0.0) {
187 if (A[index][index] < 0.0) {
188 diag_elem_vec[index] =
189 -diag_elem_vec[index];
191 for (
int row_num = index; row_num < m;
194 diag_elem_vec[index];
196 A[index][index] += 1.0;
198 diag_elem_vec[index] = -diag_elem_vec[index];
200 for (
int col_num = index + 1; col_num < n; col_num++) {
201 if ((index < col_trans_num)
202 & (diag_elem_vec[index] != 0.0)) {
206 for (
int i = index; i < m; i++) {
207 transf_val += A[i][index]
210 transf_val = -transf_val / A[index][index];
211 for (
int i = index; i < m; i++) {
212 A[i][col_num] += transf_val
216 sup_diag_elem_vec[col_num] = A[index][col_num];
218 if (index < col_trans_num) {
221 for (
int row_num = index; row_num < m; row_num++) {
222 U0[row_num][index] = A[row_num][index];
225 if (index < row_trans_num) {
226 sup_diag_elem_vec[index] = 0;
227 for (
int i = index + 1; i < n; i++) {
228 sup_diag_elem_vec[index] = hypot(
229 sup_diag_elem_vec[index],
230 sup_diag_elem_vec[i]);
232 if (sup_diag_elem_vec[index] != 0.0) {
233 if (sup_diag_elem_vec[index + 1] < 0.0) {
234 sup_diag_elem_vec[index] =
235 -sup_diag_elem_vec[index];
237 for (
int i = index + 1; i < n; i++) {
238 sup_diag_elem_vec[i] /=
239 sup_diag_elem_vec[index];
241 sup_diag_elem_vec[index + 1] += 1.0;
243 sup_diag_elem_vec[index] = -sup_diag_elem_vec[index];
245 & (sup_diag_elem_vec[index] != 0.0)) {
249 for (
int i = index + 1; i < m; i++) {
252 for (
int j = index + 1; j < n; j++) {
253 for (
int i = index + 1; i < m; i++) {
261 for (
int col_num = index + 1; col_num < n;
264 -sup_diag_elem_vec[col_num]
265 / sup_diag_elem_vec[index
267 for (
int row_num = index + 1;
270 A[row_num][col_num] +=
272 * transf_vec[row_num];
278 for (
int row_num = index + 1; row_num < n; row_num++) {
279 V0[row_num][index] = sup_diag_elem_vec[row_num];
304 static void svd_setup_bi_diagonal_matrix(uint8_t m, uint8_t n,
matrix_t A[m][n],
311 int col_trans_num = min(m - 1, n);
312 int row_trans_num = max(0, min(n - 2, m));
313 int8_t upp_col_num = min(m, n);
314 int p_index = min(n, m + 1);
316 if (col_trans_num < n) {
317 diag_elem_vec[col_trans_num] = A[col_trans_num][col_trans_num];
320 diag_elem_vec[p_index - 1] = 0.0;
322 if (row_trans_num + 1 < p_index) {
323 sup_diag_elem_vec[row_trans_num] =
324 A[row_trans_num][p_index - 1];
326 sup_diag_elem_vec[p_index - 1] = 0.0;
329 for (
int j = col_trans_num; j < upp_col_num; j++) {
330 for (
int i = 0; i < m; i++) {
335 for (
int k = col_trans_num - 1; k >= 0; k--) {
336 if (diag_elem_vec[k] != 0.0) {
337 for (
int j = k + 1; j < upp_col_num; j++) {
339 for (
int i = k; i < m; i++) {
340 factor_t += U1[i][k] * U1[i][j];
342 factor_t = -factor_t / U1[k][k];
343 for (
int i = k; i < m; i++) {
344 U1[i][j] += factor_t * U1[i][k];
347 for (
int i = k; i < m; i++) {
348 U1[i][k] = -U1[i][k];
351 for (
int i = 0; i < k - 1; i++) {
356 for (
int i = 0; i < m; i++) {
364 for (
int k = n - 1; k >= 0; k--) {
365 if ((k < row_trans_num) & (sup_diag_elem_vec[k] != 0.0)) {
366 for (
int j = k + 1; j < n; j++) {
368 for (
int i = k + 1; i < n; i++) {
369 t += V1[i][k] * V1[i][j];
371 t = -t / V1[k + 1][k];
372 for (
int i = k + 1; i < n; i++) {
373 V1[i][j] += t * V1[i][k];
377 for (
int i = 0; i < n; i++) {
406 static void svd_compute_singular_values_U_V(uint8_t m, uint8_t n, uint8_t u_n,
412 int p_index = min(n, m + 1);
413 int max_k = p_index - 1;
418 while (p_index > 0) {
419 int k_index, case_num;
421 for (k_index = p_index - 2; k_index >= -1; k_index--) {
425 if (fabs(sup_diag_elem_vec[k_index])
430 diag_elem_vec[k_index])
432 diag_elem_vec[k_index
434 sup_diag_elem_vec[k_index] = 0.0;
438 if (k_index == p_index - 2) {
443 for (ks_index = p_index - 1; ks_index >= k_index;
445 if (ks_index == k_index) {
449 (ks_index != p_index ?
451 sup_diag_elem_vec[ks_index]) :
458 sup_diag_elem_vec[ks_index
461 if (fabs(diag_elem_vec[ks_index])
463 diag_elem_vec[ks_index] = 0.0;
467 if (ks_index == k_index) {
470 else if (ks_index == p_index - 1) {
484 matrix_t f_val = sup_diag_elem_vec[p_index - 2];
485 sup_diag_elem_vec[p_index - 2] = 0.0;
486 for (
int j = p_index - 2; j >= k_index; j--) {
487 matrix_t t_val = hypot(diag_elem_vec[j], f_val);
488 matrix_t split_cs_fac = diag_elem_vec[j]
490 matrix_t split_sn_fac = f_val / t_val;
491 diag_elem_vec[j] = t_val;
493 f_val = -split_sn_fac
494 * sup_diag_elem_vec[j
496 sup_diag_elem_vec[j - 1] = split_cs_fac
497 * sup_diag_elem_vec[j
501 for (
int i = 0; i < n; i++) {
503 split_cs_fac * V[i][j]
508 -split_sn_fac * V[i][j]
520 matrix_t f_val = sup_diag_elem_vec[k_index - 1];
521 sup_diag_elem_vec[k_index - 1] = 0.0;
522 for (
int j = k_index; j < p_index; j++) {
523 matrix_t t_val = hypot(diag_elem_vec[j], f_val);
524 matrix_t split_cs_fac = diag_elem_vec[j]
526 matrix_t split_sn_fac = f_val / t_val;
527 diag_elem_vec[j] = t_val;
528 f_val = -split_sn_fac * sup_diag_elem_vec[j];
529 sup_diag_elem_vec[j] = split_cs_fac
530 * sup_diag_elem_vec[j];
533 for (
int i = 0; i < m; i++) {
535 split_cs_fac * U[i][j]
540 -split_sn_fac * U[i][j]
553 matrix_t buff[5] = { diag_elem_vec[p_index - 1],
554 diag_elem_vec[p_index - 2],
555 sup_diag_elem_vec[p_index - 2],
556 diag_elem_vec[k_index],
557 sup_diag_elem_vec[k_index] };
558 matrix_t scale = get_abs_max(buff, 5);
560 matrix_t p_diag_elem = diag_elem_vec[p_index - 1]
562 matrix_t p_minus_diag_elem = diag_elem_vec[p_index - 2]
565 sup_diag_elem_vec[p_index - 2] / scale;
566 matrix_t k_diag_elem = diag_elem_vec[k_index] / scale;
567 matrix_t k_sup_diag_elem = sup_diag_elem_vec[k_index]
569 matrix_t d_val = ((p_minus_diag_elem + p_diag_elem)
570 * (p_minus_diag_elem - p_diag_elem)
571 + p_minus_sup_diag_elem
572 * p_minus_sup_diag_elem)
574 matrix_t e_val = (p_diag_elem * p_minus_sup_diag_elem)
575 * (p_diag_elem * p_minus_sup_diag_elem);
577 if ((d_val != 0.0) | (e_val != 0.0)) {
578 wilkinson_shift = sqrt(d_val * d_val + e_val);
580 wilkinson_shift = -wilkinson_shift;
582 wilkinson_shift = e_val
583 / (d_val + wilkinson_shift);
585 matrix_t f_val = (k_diag_elem + p_diag_elem)
586 * (k_diag_elem - p_diag_elem)
588 matrix_t g_val = k_diag_elem * k_sup_diag_elem;
590 for (
int j = k_index; j < p_index - 1; j++) {
595 sup_diag_elem_vec[j - 1] = t;
598 split_cs_fac * diag_elem_vec[j]
600 * sup_diag_elem_vec[j];
601 sup_diag_elem_vec[j] =
603 * sup_diag_elem_vec[j]
606 g_val = split_sn_fac * diag_elem_vec[j + 1];
607 diag_elem_vec[j + 1] = split_cs_fac
608 * diag_elem_vec[j + 1];
611 for (
int i = 0; i < n; i++) {
613 split_cs_fac * V[i][j]
618 -split_sn_fac * V[i][j]
625 t = hypot(f_val, g_val);
626 split_cs_fac = f_val / t;
627 split_sn_fac = g_val / t;
628 diag_elem_vec[j] = t;
631 * sup_diag_elem_vec[j]
635 diag_elem_vec[j + 1] =
637 * sup_diag_elem_vec[j]
641 g_val = split_sn_fac * sup_diag_elem_vec[j + 1];
642 sup_diag_elem_vec[j + 1] = split_cs_fac
643 * sup_diag_elem_vec[j + 1];
645 for (
int i = 0; i < m; i++) {
662 sup_diag_elem_vec[p_index - 2] = f_val;
670 if (diag_elem_vec[k_index] <= 0.0) {
671 diag_elem_vec[k_index] =
672 (diag_elem_vec[k_index] < 0.0 ?
673 -diag_elem_vec[k_index] :
676 for (
int i = 0; i < n; i++) {
677 V[i][k_index] = -V[i][k_index];
681 while (k_index < max_k) {
682 if (diag_elem_vec[k_index]
683 >= diag_elem_vec[k_index + 1]) {
686 matrix_t k_diag_elem = diag_elem_vec[k_index];
687 diag_elem_vec[k_index] = diag_elem_vec[k_index
689 diag_elem_vec[k_index + 1] = k_diag_elem;
690 if (k_index < n - 1) {
691 for (
int i = 0; i < n; i++) {
692 k_diag_elem = V[i][k_index + 1];
695 V[i][k_index] = k_diag_elem;
698 if (k_index < m - 1) {
699 for (
int i = 0; i < m; i++) {
700 k_diag_elem = U[i][k_index + 1];
703 U[i][k_index] = k_diag_elem;
732 static void svd_generate_S(uint8_t m, uint8_t n,
matrix_t S[n][n],
733 uint8_t sing_vec_length,
matrix_t singl_values_vec[])
737 for (i = 0; i < m; i++) {
738 for (uint8_t j = 0; j < n; j++) {
741 if (i < sing_vec_length) {
742 S[i][i] = singl_values_vec[i];
754 matrix_t tol = max(m, n) * singl_values_arr[0] * eps;
756 for (i = 0; i < length; i++) {
757 if (fabs(singl_values_arr[i]) >= tol) {
758 recip_singl_values_arr[i] = 1.0 / singl_values_arr[i];
761 recip_singl_values_arr[i] = 0;
770 "########################## Test %d ##########################\n",
779 printf(
"matrix%d =\n", i);
786 printf(
"U%d =\n", i);
790 printf(
"S%d =\n", i);
794 printf(
"V%d =\n", i);
800 for (uint8_t i = 0; i < s_length; i++) {
801 printf(
"%5.4f ", s[i]);
802 if (i < s_length - 1) {