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) {