void eqnsys<nr_type_t>::factorize_qrh (void) {
int c, r, k, pivot;
nr_type_t f, t;
nr_double_t s, MaxPivot;
delete R; R = new tvector<nr_type_t> (N);
for (c = 0; c < N; c++) {
// compute column norms and save in work array
nPvt[c] = euclidian_c (c);
cMap[c] = c; // initialize permutation vector
}
for (c = 0; c < N; c++) {
// put column with largest norm into pivot position
MaxPivot = nPvt[c]; pivot = c;
for (r = c + 1; r < N; r++) {
if ((s = nPvt[r]) > MaxPivot) {
pivot = r;
MaxPivot = s;
}
}
if (pivot != c) {
A->exchangeCols (pivot, c);
Swap (int, cMap[pivot], cMap[c]);
Swap (nr_double_t, nPvt[pivot], nPvt[c]);
}
// compute householder vector
if (c < N) {
nr_type_t a, b;
s = euclidian_c (c, c + 1);
a = A_(c, c);
b = -sign (a) * xhypot (a, s); // Wj
t = xhypot (s, a - b); // || Vi - Wi ||
R_(c) = b;
// householder vector entries Ui
A_(c, c) = (a - b) / t;
for (r = c + 1; r < N; r++) A_(r, c) /= t;
}
else {
R_(c) = A_(c, c);
}
// apply householder transformation to remaining columns
for (r = c + 1; r < N; r++) {
for (f = 0, k = c; k < N; k++) f += cond_conj (A_(k, c)) * A_(k, r);
for (k = c; k < N; k++) A_(k, r) -= 2.0 * f * A_(k, c);
}
// update norms of remaining columns too
for (r = c + 1; r < N; r++) {
nPvt[r] = euclidian_c (r, c + 1);
}
}
}
//! Helper function computes Givens rotation.
static inline nr_double_t
givens (nr_double_t a, nr_double_t b, nr_double_t& c, nr_double_t& s) {
nr_double_t z = xhypot (a, b);
c = a / z;
s = b / z;
return z;
}
nr_type_t eqnsys<nr_type_t>::householder_create_right (int r) {
nr_type_t a, b, t;
nr_double_t s, g;
s = euclidian_r (r, r + 2);
if (s == 0 && imag (A_(r, r + 1)) == 0) {
// no reflection necessary
t = 0;
}
else {
// calculate householder reflection
a = A_(r, r + 1);
g = sign_(a) * xhypot (a, s);
b = a + g;
t = b / g;
// store householder vector
for (int c = r + 2; c < N; c++) A_(r, c) /= b;
A_(r, r + 1) = -g;
}
return t;
}
nr_type_t eqnsys<nr_type_t>::householder_create_left (int c) {
nr_type_t a, b, t;
nr_double_t s, g;
s = euclidian_c (c, c + 1);
if (s == 0 && imag (A_(c, c)) == 0) {
// no reflection necessary
t = 0;
}
else {
// calculate householder reflection
a = A_(c, c);
g = sign_(a) * xhypot (a, s);
b = a + g;
t = b / g;
// store householder vector
for (int r = c + 1; r < N; r++) A_(r, c) /= b;
A_(c, c) = -g;
}
return t;
}
void eqnsys<nr_type_t>::diagonalize_svd (void) {
bool split;
int i, l, j, its, k, n, MaxIters = 30;
nr_double_t an, f, g, h, d, c, s, b, a;
// find largest bidiagonal value
for (an = 0, i = 0; i < N; i++)
an = MAX (an, fabs (S_(i)) + fabs (E_(i)));
// diagonalize the bidiagonal matrix (stored as super-diagonal
// vector E and diagonal vector S)
for (k = N - 1; k >= 0; k--) {
// loop over singular values
for (its = 0; its <= MaxIters; its++) {
split = true;
// check for a zero entry along the super-diagonal E, if there
// is one, it is possible to QR iterate on two separate matrices
// above and below it
for (n = 0, l = k; l >= 1; l--) {
// note that E_(0) is always zero
n = l - 1;
if (fabs (E_(l)) + an == an) { split = false; break; }
if (fabs (S_(n)) + an == an) break;
}
// if there is a zero on the diagonal S, it is possible to zero
// out the corresponding super-diagonal E entry to its right
if (split) {
// cancellation of E_(l), if l > 0
c = 0.0;
s = 1.0;
for (i = l; i <= k; i++) {
f = -s * E_(i);
E_(i) *= c;
if (fabs (f) + an == an) break;
g = S_(i);
S_(i) = givens (f, g, c, s);
// apply inverse rotation to U
givens_apply_u (n, i, c, s);
}
}
d = S_(k);
// convergence
if (l == k) {
// singular value is made non-negative
if (d < 0.0) {
S_(k) = -d;
for (j = 0; j < N; j++) V_(k, j) = -V_(k, j);
}
break;
}
if (its == MaxIters) {
logprint (LOG_ERROR, "WARNING: no convergence in %d SVD iterations\n",
MaxIters);
}
// shift from bottom 2-by-2 minor
a = S_(l);
n = k - 1;
b = S_(n);
g = E_(n);
h = E_(k);
// compute QR shift value (as close as possible to the largest
// eigenvalue of the 2-by-2 minor matrix)
f = (b - d) * (b + d) + (g - h) * (g + h);
f /= 2.0 * h * b;
f += sign_(f) * xhypot (f, 1.0);
f = ((a - d) * (a + d) + h * (b / f - h)) / a;
// f => (B_{ll}^2 - u) / B_{ll}
// u => eigenvalue of T = B' * B nearer T_{22} (Wilkinson shift)
// next QR transformation
c = s = 1.0;
for (j = l; j <= n; j++) {
i = j + 1;
g = E_(i);
b = S_(i);
h = s * g; // h => right-hand non-zero to annihilate
g *= c;
E_(j) = givens (f, h, c, s);
// perform the rotation
f = a * c + g * s;
g = g * c - a * s;
h = b * s;
b *= c;
// here: +- -+
// | f g | = B * V'_j (also first V'_1)
// | h b |
// +- -+
// accumulate the rotation in V'
givens_apply_v (j, i, c, s);
d = S_(j) = xhypot (f, h);
// rotation can be arbitrary if d = 0
if (d != 0.0) {
// d => non-zero result on diagonal
d = 1.0 / d;
// rotation coefficients to annihilate the lower non-zero
c = f * d;
s = h * d;
}
f = c * g + s * b;
a = c * b - s * g;
// here: +- -+
// | d f | => U_j * B
// | 0 a |
// +- -+
// accumulate rotation into U
givens_apply_u (j, i, c, s);
}
E_(l) = 0;
E_(k) = f;
S_(k) = a;
}
}
}
/* The function computes a EMI receiver spectrum based on the given
waveform in the time domain. The number of points in the waveform
is required to be a power of two. Also the samples are supposed
to be equidistant. */
vector * emi::receiver (nr_double_t * ida, nr_double_t duration, int ilength) {
int i, n, points;
nr_double_t fres;
vector * ed = new vector ();
points = ilength;
/* ilength must be a power of 2 - write wrapper later on */
fourier::_fft_1d (ida, ilength, 1); /* 1 = forward fft */
/* start at first AC point (0 as DC point remains untouched)
additionally only half of the FFT result required */
for (i = 2; i < points; i++) {
ida[i] /= points / 2;
}
/* calculate frequency step */
fres = 1.0 / duration;
/* generate data vector; inplace calculation of magnitudes */
nr_double_t * d = ida;
for (n = 0, i = 0; i < points / 2; i++, n += 2){
/* abs value of complex number */
d[i] = xhypot (ida[n], ida[n + 1]);
/* vector contains complex values; thus every second value */
}
points /= 2;
/* define EMI settings */
struct settings settings[] = {
{ 200, 150e3, 200, 200 },
{ 150e3, 30e6, 9e3, 9e3 },
{ 30e6, 1e9, 120e3, 120e3 },
{ 0, 0, 0, 0}
};
/* define EMI noise floor */
nr_double_t noise = std::pow (10.0, (-100.0 / 40.0)) * 1e-6;
/* generate resulting data & frequency vector */
nr_double_t fcur, dcur;
int ei = 0;
/* go through each EMI setting */
for (i = 0; settings[i].bandwidth != 0; i++ ) {
nr_double_t bw = settings[i].bandwidth;
nr_double_t fstart = settings[i].start;
nr_double_t fstop = settings[i].stop;
nr_double_t fstep = settings[i].stepsize;
/* go through frequencies */
for (fcur = fstart; fcur <= fstop; fcur += fstep) {
/* calculate upper and lower frequency bounds */
nr_double_t lo = fcur - bw / 2;
nr_double_t hi = fcur + bw / 2;
if (hi < fres) continue;
/* calculate indices covering current bandwidth */
int il = std::floor (lo / fres);
int ir = std::floor (hi / fres);
/* right index (ri) greater 0 and left index less than points ->
at least part of data is within bandwidth indices */
if (ir >= 0 && il < points - 1) {
/* adjust indices to reside in the data array */
if (il < 0) il = 0;
if (ir > points - 1) ir = points - 1;
/* sum-up the values within the bandwidth */
dcur = 0;
for (int j = 0; j < ir - il; j++){
nr_double_t f = fres * (il + j);
dcur += f_2ndorder (fcur, bw, f) * d[il + j];
}
/* add noise to the result */
dcur += noise * sqrt (bw);
/* save result */
ed->add (nr_complex_t (dcur, fcur));
ei++;
}
}
}
/* returning values of function */
return ed;
}
