1 /* This function computes the solution to the least squares system
2
3 phi = [ A x = b , lambda D x = 0 ]^2
4
5 where A is an M by N matrix, D is an N by N diagonal matrix, lambda
6 is a scalar parameter and b is a vector of length M.
7
8 The function requires the factorization of A into A = Q R P^T,
9 where Q is an orthogonal matrix, R is an upper triangular matrix
10 with diagonal elements of non-increasing magnitude and P is a
11 permuation matrix. The system above is then equivalent to
12
13 [ R z = Q^T b, P^T (lambda D) P z = 0 ]
14
15 where x = P z. If this system does not have full rank then a least
16 squares solution is obtained. On output the function also provides
17 an upper triangular matrix S such that
18
19 P^T (A^T A + lambda^2 D^T D) P = S^T S
20
21 Parameters,
22
23 r: On input, contains the full upper triangle of R. The diagonal
24 elements are modified but restored on output. The full
25 upper triangle of R is not modified.
26
27 p: the encoded form of the permutation matrix P. column j of P is
28 column p[j] of the identity matrix.
29
30 lambda, diag: contains the scalar lambda and the diagonal elements
31 of the matrix D
32
33 qtb: contains the product Q^T b
34
35 S: on output contains the matrix S, n-by-n
36
37 x: on output contains the least squares solution of the system
38
39 work: is a workspace of length N
40
41 */
42
43 static int
qrsolv(gsl_matrix * r,const gsl_permutation * p,const double lambda,const gsl_vector * diag,const gsl_vector * qtb,gsl_matrix * S,gsl_vector * x,gsl_vector * work)44 qrsolv (gsl_matrix * r, const gsl_permutation * p, const double lambda,
45 const gsl_vector * diag, const gsl_vector * qtb,
46 gsl_matrix * S, gsl_vector * x, gsl_vector * work)
47 {
48 size_t n = r->size2;
49
50 size_t i, j, k, nsing;
51
52 /* Copy r and qtb to preserve input and initialise s. In particular,
53 save the diagonal elements of r in x */
54
55 for (j = 0; j < n; j++)
56 {
57 double rjj = gsl_matrix_get (r, j, j);
58 double qtbj = gsl_vector_get (qtb, j);
59
60 for (i = j + 1; i < n; i++)
61 {
62 double rji = gsl_matrix_get (r, j, i);
63 gsl_matrix_set (S, i, j, rji);
64 }
65
66 gsl_vector_set (x, j, rjj);
67 gsl_vector_set (work, j, qtbj);
68 }
69
70 /* Eliminate the diagonal matrix d using a Givens rotation */
71
72 for (j = 0; j < n; j++)
73 {
74 double qtbpj;
75
76 size_t pj = gsl_permutation_get (p, j);
77
78 double diagpj = lambda * gsl_vector_get (diag, pj);
79
80 if (diagpj == 0)
81 {
82 continue;
83 }
84
85 gsl_matrix_set (S, j, j, diagpj);
86
87 for (k = j + 1; k < n; k++)
88 {
89 gsl_matrix_set (S, k, k, 0.0);
90 }
91
92 /* The transformations to eliminate the row of d modify only a
93 single element of qtb beyond the first n, which is initially
94 zero */
95
96 qtbpj = 0;
97
98 for (k = j; k < n; k++)
99 {
100 /* Determine a Givens rotation which eliminates the
101 appropriate element in the current row of d */
102
103 double sine, cosine;
104
105 double wk = gsl_vector_get (work, k);
106 double rkk = gsl_matrix_get (r, k, k);
107 double skk = gsl_matrix_get (S, k, k);
108
109 if (skk == 0)
110 {
111 continue;
112 }
113
114 if (fabs (rkk) < fabs (skk))
115 {
116 double cotangent = rkk / skk;
117 sine = 0.5 / sqrt (0.25 + 0.25 * cotangent * cotangent);
118 cosine = sine * cotangent;
119 }
120 else
121 {
122 double tangent = skk / rkk;
123 cosine = 0.5 / sqrt (0.25 + 0.25 * tangent * tangent);
124 sine = cosine * tangent;
125 }
126
127 /* Compute the modified diagonal element of r and the
128 modified element of [qtb,0] */
129
130 {
131 double new_rkk = cosine * rkk + sine * skk;
132 double new_wk = cosine * wk + sine * qtbpj;
133
134 qtbpj = -sine * wk + cosine * qtbpj;
135
136 gsl_matrix_set(r, k, k, new_rkk);
137 gsl_matrix_set(S, k, k, new_rkk);
138 gsl_vector_set(work, k, new_wk);
139 }
140
141 /* Accumulate the transformation in the row of s */
142
143 for (i = k + 1; i < n; i++)
144 {
145 double sik = gsl_matrix_get (S, i, k);
146 double sii = gsl_matrix_get (S, i, i);
147
148 double new_sik = cosine * sik + sine * sii;
149 double new_sii = -sine * sik + cosine * sii;
150
151 gsl_matrix_set(S, i, k, new_sik);
152 gsl_matrix_set(S, i, i, new_sii);
153 }
154 }
155
156 /* Store the corresponding diagonal element of s and restore the
157 corresponding diagonal element of r */
158
159 {
160 double xj = gsl_vector_get(x, j);
161 gsl_matrix_set (r, j, j, xj);
162 }
163
164 }
165
166 /* Solve the triangular system for z. If the system is singular then
167 obtain a least squares solution */
168
169 nsing = n;
170
171 for (j = 0; j < n; j++)
172 {
173 double sjj = gsl_matrix_get (S, j, j);
174
175 if (sjj == 0)
176 {
177 nsing = j;
178 break;
179 }
180 }
181
182 for (j = nsing; j < n; j++)
183 {
184 gsl_vector_set (work, j, 0.0);
185 }
186
187 for (k = 0; k < nsing; k++)
188 {
189 double sum = 0;
190
191 j = (nsing - 1) - k;
192
193 for (i = j + 1; i < nsing; i++)
194 {
195 sum += gsl_matrix_get(S, i, j) * gsl_vector_get(work, i);
196 }
197
198 {
199 double wj = gsl_vector_get (work, j);
200 double sjj = gsl_matrix_get (S, j, j);
201
202 gsl_vector_set (work, j, (wj - sum) / sjj);
203 }
204 }
205
206 /* Permute the components of z back to the components of x */
207
208 for (j = 0; j < n; j++)
209 {
210 size_t pj = gsl_permutation_get (p, j);
211 double wj = gsl_vector_get (work, j);
212
213 gsl_vector_set (x, pj, wj);
214 }
215
216 return GSL_SUCCESS;
217 }
218