1 #include "scaling.h"
2
3 #if EMBEDDED != 1
4
5
6 // Set values lower than threshold SCALING_REG to 1
limit_scaling(c_float * D,c_int n)7 void limit_scaling(c_float *D, c_int n) {
8 c_int i;
9
10 for (i = 0; i < n; i++) {
11 D[i] = D[i] < MIN_SCALING ? 1.0 : D[i];
12 D[i] = D[i] > MAX_SCALING ? MAX_SCALING : D[i];
13 }
14 }
15
16 /**
17 * Compute infinite norm of the columns of the KKT matrix without forming it
18 *
19 * The norm is stored in the vector v = (D, E)
20 *
21 * @param P Cost matrix
22 * @param A Constraints matrix
23 * @param D Norm of columns related to variables
24 * @param D_temp_A Temporary vector for norm of columns of A
25 * @param E Norm of columns related to constraints
26 * @param n Dimension of KKT matrix
27 */
compute_inf_norm_cols_KKT(const csc * P,const csc * A,c_float * D,c_float * D_temp_A,c_float * E,c_int n)28 void compute_inf_norm_cols_KKT(const csc *P, const csc *A,
29 c_float *D, c_float *D_temp_A,
30 c_float *E, c_int n) {
31 // First half
32 // [ P ]
33 // [ A ]
34 mat_inf_norm_cols_sym_triu(P, D);
35 mat_inf_norm_cols(A, D_temp_A);
36 vec_ew_max_vec(D, D_temp_A, D, n);
37
38 // Second half
39 // [ A']
40 // [ 0 ]
41 mat_inf_norm_rows(A, E);
42 }
43
scale_data(OSQPWorkspace * work)44 c_int scale_data(OSQPWorkspace *work) {
45 // Scale KKT matrix
46 //
47 // [ P A']
48 // [ A 0 ]
49 //
50 // with diagonal matrix
51 //
52 // S = [ D ]
53 // [ E ]
54 //
55
56 c_int i; // Iterations index
57 c_int n, m; // Number of constraints and variables
58 c_float c_temp; // Cost function scaling
59 c_float inf_norm_q; // Infinity norm of q
60
61 n = work->data->n;
62 m = work->data->m;
63
64 // Initialize scaling to 1
65 work->scaling->c = 1.0;
66 vec_set_scalar(work->scaling->D, 1., work->data->n);
67 vec_set_scalar(work->scaling->Dinv, 1., work->data->n);
68 vec_set_scalar(work->scaling->E, 1., work->data->m);
69 vec_set_scalar(work->scaling->Einv, 1., work->data->m);
70
71
72 for (i = 0; i < work->settings->scaling; i++) {
73 //
74 // First Ruiz step
75 //
76
77 // Compute norm of KKT columns
78 compute_inf_norm_cols_KKT(work->data->P, work->data->A,
79 work->D_temp, work->D_temp_A,
80 work->E_temp, n);
81
82 // Set to 1 values with 0 norms (avoid crazy scaling)
83 limit_scaling(work->D_temp, n);
84 limit_scaling(work->E_temp, m);
85
86 // Take square root of norms
87 vec_ew_sqrt(work->D_temp, n);
88 vec_ew_sqrt(work->E_temp, m);
89
90 // Divide scalings D and E by themselves
91 vec_ew_recipr(work->D_temp, work->D_temp, n);
92 vec_ew_recipr(work->E_temp, work->E_temp, m);
93
94 // Equilibrate matrices P and A and vector q
95 // P <- DPD
96 mat_premult_diag(work->data->P, work->D_temp);
97 mat_postmult_diag(work->data->P, work->D_temp);
98
99 // A <- EAD
100 mat_premult_diag(work->data->A, work->E_temp);
101 mat_postmult_diag(work->data->A, work->D_temp);
102
103 // q <- Dq
104 vec_ew_prod(work->D_temp, work->data->q, work->data->q, n);
105
106 // Update equilibration matrices D and E
107 vec_ew_prod(work->scaling->D, work->D_temp, work->scaling->D, n);
108 vec_ew_prod(work->scaling->E, work->E_temp, work->scaling->E, m);
109
110 //
111 // Cost normalization step
112 //
113
114 // Compute avg norm of cols of P
115 mat_inf_norm_cols_sym_triu(work->data->P, work->D_temp);
116 c_temp = vec_mean(work->D_temp, n);
117
118 // Compute inf norm of q
119 inf_norm_q = vec_norm_inf(work->data->q, n);
120
121 // If norm_q == 0, set it to 1 (ignore it in the scaling)
122 // NB: Using the same function as with vectors here
123 limit_scaling(&inf_norm_q, 1);
124
125 // Compute max between avg norm of cols of P and inf norm of q
126 c_temp = c_max(c_temp, inf_norm_q);
127
128 // Limit scaling (use same function as with vectors)
129 limit_scaling(&c_temp, 1);
130
131 // Invert scaling c = 1 / cost_measure
132 c_temp = 1. / c_temp;
133
134 // Scale P
135 mat_mult_scalar(work->data->P, c_temp);
136
137 // Scale q
138 vec_mult_scalar(work->data->q, c_temp, n);
139
140 // Update cost scaling
141 work->scaling->c *= c_temp;
142 }
143
144
145 // Store cinv, Dinv, Einv
146 work->scaling->cinv = 1. / work->scaling->c;
147 vec_ew_recipr(work->scaling->D, work->scaling->Dinv, work->data->n);
148 vec_ew_recipr(work->scaling->E, work->scaling->Einv, work->data->m);
149
150
151 // Scale problem vectors l, u
152 vec_ew_prod(work->scaling->E, work->data->l, work->data->l, work->data->m);
153 vec_ew_prod(work->scaling->E, work->data->u, work->data->u, work->data->m);
154
155 return 0;
156 }
157
158 #endif // EMBEDDED
159
unscale_data(OSQPWorkspace * work)160 c_int unscale_data(OSQPWorkspace *work) {
161 // Unscale cost
162 mat_mult_scalar(work->data->P, work->scaling->cinv);
163 mat_premult_diag(work->data->P, work->scaling->Dinv);
164 mat_postmult_diag(work->data->P, work->scaling->Dinv);
165 vec_mult_scalar(work->data->q, work->scaling->cinv, work->data->n);
166 vec_ew_prod(work->scaling->Dinv, work->data->q, work->data->q, work->data->n);
167
168 // Unscale constraints
169 mat_premult_diag(work->data->A, work->scaling->Einv);
170 mat_postmult_diag(work->data->A, work->scaling->Dinv);
171 vec_ew_prod(work->scaling->Einv, work->data->l, work->data->l, work->data->m);
172 vec_ew_prod(work->scaling->Einv, work->data->u, work->data->u, work->data->m);
173
174 return 0;
175 }
176
unscale_solution(OSQPWorkspace * work)177 c_int unscale_solution(OSQPWorkspace *work) {
178 // primal
179 vec_ew_prod(work->scaling->D,
180 work->solution->x,
181 work->solution->x,
182 work->data->n);
183
184 // dual
185 vec_ew_prod(work->scaling->E,
186 work->solution->y,
187 work->solution->y,
188 work->data->m);
189 vec_mult_scalar(work->solution->y, work->scaling->cinv, work->data->m);
190
191 return 0;
192 }
193