void RSL_rebin_ray(Ray *r, int width)
{
int i;
float nyquist, val;
int ncbins = 15; /* Number of color bins */
float (*f)(Range x);
Range (*invf)(float x);
if (r == NULL) return;
nyquist = width;
if (nyquist == 0.0) {
fprintf(stderr, "RSL_rebin_ray: nyquist == 0.0\n");
fprintf(stderr, "RSL_rebin_ray: Unable to rebin.\n");
return;
}
f = r->h.f;
invf = r->h.invf;
for (i=0; i<r->h.nbins; i++) {
val = f(r->range[i]);
if (val == width+1) {
val = ncbins + 1;
} else if (val != BADVAL) {
/*
Okay, we want to shift the data to positive values
then we re-scale them by the number of color bins/nyquist
*/
val = (int)(val/nyquist*(ncbins/2) + 1.0 + ncbins/2);
} else {
val = 0;
}
r->range[i] = invf(val);
}
}
Ray *RSL_ray_z_to_r(Ray *z_ray, float k, float a)
{
Ray *r_ray;
int i;
Range (*invf)(float x);
float (*f)(Range x);
if (z_ray == NULL) return NULL;
r_ray = RSL_new_ray(z_ray->h.nbins);
r_ray->h = z_ray->h;
f = r_ray->h.f;
invf = r_ray->h.invf;
for(i=0; i<z_ray->h.nbins; i++) {
r_ray->range[i] = invf(RSL_z_to_r( f(z_ray->range[i]), k, a));
}
return r_ray;
}
void RSL_rebin_zdr_ray(Ray *r)
{
int i;
float val;
float (*f)(Range x);
Range (*invf)(float x);
if (r == NULL) return;
f = r->h.f;
invf = r->h.invf;
for (i=0; i<r->h.nbins; i++)
{
val = f(r->range[i]);
if ((val >= -6.0) && (val < 8.4)) val = (floor) ((val + 6.0) * 2.5);
else if (val < 10.0) val = 35.0; /* Make all these white. */
else val = 0; /* invalid zdr value */
r->range[i] = invf(val);
}
}
void RSL_rebin_velocity_ray(Ray *r)
{
/* Rebin the velocity data to the range -nyquist, +nyquist.
* 14 bins are created centered at 0. It sets the proper color look up
* indexes. This function modifies Ray r.
*/
int i;
float nyquist;
float val /* val1 */;
int ncbins = 15; /* Number of color bins */
float (*f)(Range x);
Range (*invf)(float x);
if (r == NULL) return;
nyquist = r->h.nyq_vel;
if (nyquist == 0.0) {
fprintf(stderr, "RSL_rebin_velocity_ray: nyquist == 0.0\n");
fprintf(stderr, "RSL_rebin_velocity_ray: Unable to rebin.\n");
return;
}
f = r->h.f;
invf = r->h.invf;
for (i=0; i<r->h.nbins; i++) {
val = f(r->range[i]);
if (val == RFVAL) {
val = 16;
} else if (val != BADVAL) {
/*
Okay, we want to shift the data to positive values
then we re-scale them by the number of color bins/nyquist
*/
val = (int)(val/nyquist*(ncbins/2) + 1.0 + ncbins/2);
} else {
val = 0;
}
r->range[i] = invf(val);
}
}
void KFKSDS_deriv_C (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0, double *dvof,
double *epshat, double *vareps, double *etahat, double *vareta,
double *r, double *N, double *dr, double *dN,
double *dahat, double *dvareps)
{
//int s, p = dim[1], mp1 = m + 1;
int i, ip1, j, k, n = dim[0], m = dim[2],
ir = dim[3], rp1 = ir + 1, nrp1 = n * rp1,
rp1m = rp1 * m, iaux, irp1m,
irsod = ir * sizeof(double), msod = m * sizeof(double),
nsod = n * sizeof(double), rp1msod = rp1 * msod;
//double invf[n], vof[n], msHsq, dfinvfsq[nrp1];
double msHsq;
std::vector<double> invf(n);
std::vector<double> vof(n);
std::vector<double> dfinvfsq(nrp1);
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_vector * Z_cp = gsl_vector_alloc(m);
gsl_matrix * ZtZ = gsl_matrix_alloc(m, m);
gsl_matrix_view maux1, maux2;
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), m, 1);
gsl_vector_memcpy(Z_cp, &Z.vector);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&maux2.matrix, 0.0, ZtZ);
gsl_matrix * a_pred = gsl_matrix_alloc(n, m);
std::vector<gsl_matrix*> P_pred(n);
gsl_matrix * K = gsl_matrix_alloc(n, m);
gsl_vector_view K_irow;
std::vector<gsl_matrix*> L(n);
gsl_vector_view Qdiag = gsl_matrix_diagonal(&Q.matrix);
gsl_vector * Qdiag_msq = gsl_vector_alloc(m);
gsl_vector_memcpy(Qdiag_msq, &Qdiag.vector);
gsl_vector_mul(Qdiag_msq, &Qdiag.vector);
gsl_vector_scale(Qdiag_msq, -1.0);
std::vector<gsl_matrix*> da_pred(rp1);
std::vector< std::vector<gsl_matrix*> > dP_pred(n, std::vector<gsl_matrix*>(rp1));
std::vector<gsl_matrix*> dK(n);
// filtering
KF_deriv_aux_C(dim, sy, sZ, sT, sH, sR, sV, sQ, sa0, sP0,
&invf, &vof, dvof, &dfinvfsq, a_pred, &P_pred, K,
&L, &da_pred, &dP_pred, &dK);
// state vector smoothing and disturbances smoothing
gsl_matrix_view V = gsl_matrix_view_array(sV, ir, ir);
gsl_matrix_view R = gsl_matrix_view_array(sR, m, ir);
gsl_vector_view vaux;
gsl_vector *vaux2 = gsl_vector_alloc(m);
gsl_matrix *Mmm = gsl_matrix_alloc(m, m);
gsl_matrix *Mmm2 = gsl_matrix_alloc(m, m);
gsl_matrix *Mrm = gsl_matrix_alloc(ir, m);
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_matrix *r0 = gsl_matrix_alloc(n + 1, m);
gsl_vector_view r_row_t;
gsl_vector_view r_row_tp1 = gsl_matrix_row(r0, n);
gsl_vector_set_zero(&r_row_tp1.vector);
std::vector<gsl_matrix*> N0(n + 1);
N0.at(n) = gsl_matrix_calloc(m, m);
gsl_vector_view Ndiag;
gsl_vector *var_eps = gsl_vector_alloc(n);
msHsq = -1.0 * pow(*sH, 2);
//vaux = gsl_vector_view_array(invf, n);
vaux = gsl_vector_view_array(&invf[0], n);
gsl_vector_set_all(var_eps, msHsq);
gsl_vector_mul(var_eps, &vaux.vector);
gsl_vector_add_constant(var_eps, *sH);
gsl_vector *vr = gsl_vector_alloc(ir);
gsl_matrix *dL = gsl_matrix_alloc(m, m);
std::vector<gsl_matrix*> dr0(n + 1);
dr0.at(n) = gsl_matrix_calloc(rp1, m);
gsl_vector_view dr_row_t, dr_row_tp1;
std::vector< std::vector<gsl_matrix*> > dN0(n + 1, std::vector<gsl_matrix*>(rp1));
for (j = 0; j < rp1; j++)
{
(dN0.at(n)).at(j) = gsl_matrix_calloc(m, m);
}
for (i = n-1; i > -1; i--)
{
ip1 = i + 1;
iaux = (i-1) * rp1m;
irp1m = i * rp1m;
if (i != n-1) //the case i=n-1 was initialized above
r_row_tp1 = gsl_matrix_row(r0, ip1);
r_row_t = gsl_matrix_row(r0, i);
gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &r_row_tp1.vector,
0.0, &r_row_t.vector);
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_vector_scale(Z_cp, vof.at(i));
gsl_vector_add(&r_row_t.vector, Z_cp);
gsl_vector_memcpy(vaux2, &r_row_tp1.vector);
memcpy(&r[i * m], vaux2->data, msod);
N0.at(i) = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy(N0.at(i), ZtZ);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), invf.at(i), N0.at(i));
vaux = gsl_matrix_diagonal(N0.at(ip1));
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&N[i * m], vaux2->data, msod);
K_irow = gsl_matrix_row(K, i);
gsl_blas_ddot(&K_irow.vector, &r_row_tp1.vector, &epshat[i]);
epshat[i] -= vof.at(i);
epshat[i] *= -*sH;
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), 1, m);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, N0.at(ip1),
0.0, &maux2.matrix);
vaux = gsl_vector_view_array(gsl_vector_ptr(var_eps, i), 1);
gsl_blas_dgemv(CblasNoTrans, msHsq, &maux2.matrix, &K_irow.vector,
1.0, &vaux.vector);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &V.matrix, &R.matrix,
0.0, Mrm);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mrm, &r_row_tp1.vector,
0.0, vr);
memcpy(&etahat[i*ir], vr->data, irsod);
Ndiag = gsl_matrix_diagonal(N0.at(ip1));
gsl_vector_memcpy(Z_cp, &Ndiag.vector);
gsl_vector_mul(Z_cp, Qdiag_msq);
gsl_vector_add(Z_cp, &Qdiag.vector);
gsl_blas_dgemv(CblasTrans, 1.0, &R.matrix, Z_cp, 0.0, vr);
memcpy(&vareta[i*ir], vr->data, irsod);
// derivatives
dr0.at(i) = gsl_matrix_alloc(rp1, m);
for (j = 0; j < rp1; j++)
{
k = i + j * n;
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_vector_scale(Z_cp, dvof[k]);
vaux = gsl_matrix_row(dK.at(i), j);
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&vaux.vector, 0), m, 1);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &maux1.matrix,
&maux2.matrix, 0.0, dL);
dr_row_t = gsl_matrix_row(dr0.at(i), j);
dr_row_tp1 = gsl_matrix_row(dr0.at(ip1), j);
gsl_blas_dgemv(CblasTrans, 1.0, dL, &r_row_tp1.vector, 0.0, &dr_row_t.vector);
gsl_vector_add(&dr_row_t.vector, Z_cp);
gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &dr_row_tp1.vector, 1.0, &dr_row_t.vector);
(dN0.at(i)).at(j) = gsl_matrix_alloc(m, m);
gsl_matrix_memcpy((dN0.at(i)).at(j), ZtZ);
gsl_matrix_scale((dN0.at(i)).at(j), -1.0 * dfinvfsq.at(k));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, dL, N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i),
1.0, (dN0.at(i)).at(j));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i),
(dN0.at(ip1)).at(j), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i),
1.0, (dN0.at(i)).at(j));
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i),
N0.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, dL,
1.0, (dN0.at(i)).at(j));
if (i != 0)
{
vaux = gsl_matrix_diagonal((dN0.at(i)).at(j));
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&dN[iaux + j * m], vaux2->data, msod);
}
vaux = gsl_matrix_row(da_pred.at(j), i);
gsl_blas_dgemv(CblasNoTrans, 1.0, (dP_pred.at(i)).at(j) , &r_row_t.vector,
1.0, &vaux.vector);
gsl_blas_dgemv(CblasNoTrans, 1.0, P_pred.at(i), &dr_row_t.vector,
1.0, &vaux.vector);
gsl_vector_memcpy(vaux2, &vaux.vector);
memcpy(&dahat[irp1m + j * m], vaux2->data, msod);
gsl_matrix_memcpy(Mmm, (dP_pred.at(i)).at(j));
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, (dP_pred.at(i)).at(j),
N0.at(i), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
1.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i),
(dN0.at(i)).at(j), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2, P_pred.at(i),
1.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, P_pred.at(i),
N0.at(i), 0.0, Mmm2);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm2,
(dP_pred.at(i)).at(j), 1.0, Mmm);
gsl_matrix_mul_elements(Mmm, ZtZ);
std::vector<double> vmm(Mmm->data, Mmm->data + m*m);
dvareps[i*rp1 + j] = std::accumulate(vmm.begin(), vmm.end(), 0.0);
gsl_matrix_free((dN0.at(ip1)).at(j));
gsl_matrix_free((dP_pred.at(i)).at(j));
}
if (i != 0)
{
memcpy(&dr[iaux], (dr0.at(i))->data, rp1msod);
}
gsl_matrix_free(dr0.at(ip1));
gsl_matrix_free(dK.at(i));
gsl_matrix_free(P_pred.at(i));
gsl_matrix_free(L.at(i));
gsl_matrix_free(N0.at(ip1));
}
gsl_matrix_free(N0.at(0));
gsl_matrix_free(dr0.at(0));
for (j = 0; j < rp1; j++)
{
gsl_matrix_free((dN0.at(0)).at(j));
gsl_matrix_free(da_pred.at(j));
}
memcpy(&vareps[0], var_eps->data, nsod);
gsl_matrix_free(Mmm);
gsl_matrix_free(Mmm2);
gsl_matrix_free(Mrm);
gsl_matrix_free(r0);
gsl_matrix_free(K);
gsl_matrix_free(dL);
gsl_matrix_free(a_pred);
gsl_vector_free(Z_cp);
gsl_matrix_free(ZtZ);
gsl_vector_free(var_eps);
gsl_vector_free(vr);
gsl_vector_free(Qdiag_msq);
gsl_vector_free(vaux2);
}}
int uf_into_radar(UF_buffer uf, Radar **the_radar)
{
/* Missing data flag : -32768 when a signed short. */
#define UF_NO_DATA 0X8000
/* Any convensions may be observed, however, Radial Velocity must be VE. */
/* Typically:
* DM = Reflectivity (dB(mW)).
* DZ = Reflectivity (dBZ).
* VR = Radial Velocity.
* CZ = Corrected Reflectivity. (Quality controlled: AP removed, etc.)
* SW = Spectrum Width.
* DR = Differential Reflectivity.
* XZ = X-band Reflectivity.
*
* These fields may appear in any order in the UF file.
*
* RETURN:
* UF_DONE if we're done with the UF ingest.
* UF_MORE if we need more UF data.
*/
/* These are pointers to various locations within the UF buffer 'uf'.
* They are used to index the different components of the UF structure in
* a manor consistant with the UF documentation. For instance, uf_ma[1]
* will be equivalenced to the second word (2 bytes/each) of the UF
* buffer.
*/
short *uf_ma; /* Mandatory header block. */
short *uf_lu; /* Local Use header block. */
short *uf_dh; /* Data header. */
short *uf_fh; /* Field header. */
short *uf_data; /* Data. */
/* The length of each header. */
int len_data, len_lu;
int current_fh_index;
float scale_factor;
int nfields, isweep, ifield, iray, i, m;
static int pulled_time_from_first_ray = 0;
char *field_type; /* For printing the field type upon error. */
short proj_name[4];
Ray *ray;
Sweep *sweep;
Radar *radar;
float x;
short missing_data;
Volume *new_volume;
int nbins;
extern int rsl_qfield[];
extern int *rsl_qsweep; /* See RSL_read_these_sweeps in volume.c */
extern int rsl_qsweep_max;
radar = *the_radar;
/*
* The organization of the Radar structure is by volumes, then sweeps, then
* rays, then gates. This is different from the UF file organization.
* The UF format is sweeps, rays, then gates for all field types (volumes).
*/
/* Set up all the UF pointers. */
uf_ma = uf;
uf_lu = uf + uf_ma[3] - 1;
uf_dh = uf + uf_ma[4] - 1;
nfields = uf_dh[0];
isweep = uf_ma[9] - 1;
if (rsl_qsweep != NULL) {
if (isweep > rsl_qsweep_max) return UF_DONE;
if (rsl_qsweep[isweep] == 0) return UF_MORE;
}
/* Here is a sticky part. We must make sure that if we encounter any
* additional fields that were not previously present, that we are able
* to load them. This will require us to copy the entire radar structure
* and whack off the old one. But, we must be sure that it really is a
* new field. This is not so trivial as a couple of lines of code; I will
* have to think about this a little bit more. See STICKYSOLVED below.
*/
#ifdef STICKYSOLVED
if (radar == NULL) radar = RSL_new_radar(nfields);
/* Sticky solution here. */
#else
if (radar == NULL) {
radar = RSL_new_radar(MAX_RADAR_VOLUMES);
*the_radar = radar;
pulled_time_from_first_ray = 0;
for (i=0; i<MAX_RADAR_VOLUMES; i++)
if (rsl_qfield[i]) /* See RSL_select_fields in volume.c */
radar->v[i] = RSL_new_volume(20);
}
#endif
/* For LITTLE ENDIAN:
* WE "UNSWAP" character strings. Because there are so few of them,
* it is easier to catch them here. The entire UF buffer is swapped prior
* to entry to here, therefore, undo-ing these swaps; sets the
* character strings right.
*/
for (i=0; i<nfields; i++) {
if (little_endian()) swap_2_bytes(&uf_dh[3+2*i]); /* Unswap. */
if (strncmp((char *)&uf_dh[3+2*i], "DZ", 2) == 0) ifield = DZ_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "VR", 2) == 0) ifield = VR_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "SW", 2) == 0) ifield = SW_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "CZ", 2) == 0) ifield = CZ_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "UZ", 2) == 0) ifield = ZT_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "ZT", 2) == 0) ifield = ZT_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "DR", 2) == 0) ifield = DR_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "ZD", 2) == 0) ifield = ZD_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "DM", 2) == 0) ifield = DM_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "RH", 2) == 0) ifield = RH_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "PH", 2) == 0) ifield = PH_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "XZ", 2) == 0) ifield = XZ_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "CD", 2) == 0) ifield = CD_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "MZ", 2) == 0) ifield = MZ_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "MD", 2) == 0) ifield = MD_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "ZE", 2) == 0) ifield = ZE_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "VE", 2) == 0) ifield = VE_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "KD", 2) == 0) ifield = KD_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "TI", 2) == 0) ifield = TI_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "DX", 2) == 0) ifield = DX_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "CH", 2) == 0) ifield = CH_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "AH", 2) == 0) ifield = AH_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "CV", 2) == 0) ifield = CV_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "AV", 2) == 0) ifield = AV_INDEX;
else if (strncmp((char *)&uf_dh[3+2*i], "SQ", 2) == 0) ifield = SQ_INDEX;
else { /* etc. DON'T know how to handle this yet. */
field_type = (char *)&uf_dh[3+2*i];
fprintf(stderr, "Unknown field type %c%c\n", (char)field_type[0], (char)field_type[1]);
continue;
}
switch (ifield) {
case DZ_INDEX: f = DZ_F; invf = DZ_INVF; break;
case VR_INDEX: f = VR_F; invf = VR_INVF; break;
case SW_INDEX: f = SW_F; invf = SW_INVF; break;
case CZ_INDEX: f = CZ_F; invf = CZ_INVF; break;
case ZT_INDEX: f = ZT_F; invf = ZT_INVF; break;
case DR_INDEX: f = DR_F; invf = DR_INVF; break;
case ZD_INDEX: f = ZD_F; invf = ZD_INVF; break;
case DM_INDEX: f = DM_F; invf = DM_INVF; break;
case RH_INDEX: f = RH_F; invf = RH_INVF; break;
case PH_INDEX: f = PH_F; invf = PH_INVF; break;
case XZ_INDEX: f = XZ_F; invf = XZ_INVF; break;
case CD_INDEX: f = CD_F; invf = CD_INVF; break;
case MZ_INDEX: f = MZ_F; invf = MZ_INVF; break;
case MD_INDEX: f = MD_F; invf = MD_INVF; break;
case ZE_INDEX: f = ZE_F; invf = ZE_INVF; break;
case VE_INDEX: f = VE_F; invf = VE_INVF; break;
case KD_INDEX: f = KD_F; invf = KD_INVF; break;
case TI_INDEX: f = TI_F; invf = TI_INVF; break;
case DX_INDEX: f = DX_F; invf = DX_INVF; break;
case CH_INDEX: f = CH_F; invf = CH_INVF; break;
case AH_INDEX: f = AH_F; invf = AH_INVF; break;
case CV_INDEX: f = CV_F; invf = CV_INVF; break;
case AV_INDEX: f = AV_F; invf = AV_INVF; break;
case SQ_INDEX: f = SQ_F; invf = SQ_INVF; break;
default: f = DZ_F; invf = DZ_INVF; break;
}
/* Do we place the data into this volume? */
if (radar->v[ifield] == NULL) continue; /* Nope. */
if (isweep >= radar->v[ifield]->h.nsweeps) { /* Exceeded sweep limit.
* Allocate more sweeps.
* Copy all previous sweeps.
*/
if (radar_verbose_flag)
fprintf(stderr,"Exceeded sweep allocation of %d. Adding 20 more.\n", isweep);
new_volume = RSL_new_volume(radar->v[ifield]->h.nsweeps+20);
new_volume = copy_sweeps_into_volume(new_volume, radar->v[ifield]);
radar->v[ifield] = new_volume;
}
if (radar->v[ifield]->sweep[isweep] == NULL) {
if (radar_verbose_flag)
fprintf(stderr,"Allocating new sweep for field %d, isweep %d\n", ifield, isweep);
radar->v[ifield]->sweep[isweep] = RSL_new_sweep(1000);
radar->v[ifield]->sweep[isweep]->h.nrays = 0; /* Increment this for each
* ray encountered.
*/
radar->v[ifield]->h.f = f;
radar->v[ifield]->h.invf = invf;
radar->v[ifield]->sweep[isweep]->h.f = f;
radar->v[ifield]->sweep[isweep]->h.invf = invf;
radar->v[ifield]->sweep[isweep]->h.sweep_num = uf_ma[9];
radar->v[ifield]->sweep[isweep]->h.elev = uf_ma[35] / 64.0;
}
current_fh_index = uf_dh[4+2*i];
uf_fh = uf + current_fh_index - 1;
sweep = radar->v[ifield]->sweep[isweep];
iray = sweep->h.nrays;
nbins = uf_fh[5];
radar->v[ifield]->sweep[isweep]->ray[iray] = RSL_new_ray(nbins);
ray = radar->v[ifield]->sweep[isweep]->ray[iray];
sweep->h.nrays += 1;
if (ray) {
/*
* ---- Beginning of MANDATORY HEADER BLOCK.
*/
ray->h.ray_num = uf_ma[7];
if (little_endian()) swap2(&uf_ma[10], 8);
memcpy(radar->h.radar_name, &uf_ma[10], 8);
if (little_endian()) swap2(&uf_ma[10], 8/2);
memcpy(radar->h.name, &uf_ma[14], 8);
if (little_endian()) swap2(&uf_ma[14], 8/2);
/* All components of lat/lon are the same sign. If not, then
* what ever wrote the UF was in error. A simple RSL program
* can repair the damage, however, not here.
*/
ray->h.lat = uf_ma[18] + uf_ma[19]/60.0 + uf_ma[20]/64.0/3600;
ray->h.lon = uf_ma[21] + uf_ma[22]/60.0 + uf_ma[23]/64.0/3600;
ray->h.alt = uf_ma[24];
ray->h.year = uf_ma[25];
if (ray->h.year < 1900) {
ray->h.year += 1900;
if (ray->h.year < 1980) ray->h.year += 100; /* Year >= 2000. */
}
ray->h.month = uf_ma[26];
ray->h.day = uf_ma[27];
ray->h.hour = uf_ma[28];
ray->h.minute = uf_ma[29];
ray->h.sec = uf_ma[30];
ray->h.azimuth = uf_ma[32] / 64.0;
/* If Local Use Header is present and contains azimuth, use that
* azimuth for VR and SW. This is for WSR-88D, which runs separate
* scans for DZ and VR/SW at the lower elevations, which means DZ
* VR/SW and have different azimuths in the "same" ray.
*/
len_lu = uf_ma[4] - uf_ma[3];
if (len_lu == 2 && (ifield == VR_INDEX || ifield == SW_INDEX)) {
if (strncmp(uf_lu,"ZA",2) == 0 || strncmp(uf_lu,"AZ",2) == 0)
ray->h.azimuth = uf_lu[1] / 64.0;
}
if (ray->h.azimuth < 0.) ray->h.azimuth += 360.; /* make it 0 to 360. */
ray->h.elev = uf_ma[33] / 64.0;
ray->h.elev_num = sweep->h.sweep_num;
ray->h.fix_angle = sweep->h.elev = uf_ma[35] / 64.0;
ray->h.azim_rate = uf_ma[36] / 64.0;
ray->h.sweep_rate = ray->h.azim_rate * (60.0/360.0);
missing_data = uf_ma[44];
if (pulled_time_from_first_ray == 0) {
radar->h.height = uf_ma[24];
radar->h.latd = uf_ma[18];
radar->h.latm = uf_ma[19];
radar->h.lats = uf_ma[20] / 64.0;
radar->h.lond = uf_ma[21];
radar->h.lonm = uf_ma[22];
radar->h.lons = uf_ma[23] / 64.0;
radar->h.year = ray->h.year;
radar->h.month = ray->h.month;
radar->h.day = ray->h.day;
radar->h.hour = ray->h.hour;
radar->h.minute = ray->h.minute;
radar->h.sec = ray->h.sec;
strcpy(radar->h.radar_type, "uf");
pulled_time_from_first_ray = 1;
}
/*
* ---- End of MANDATORY HEADER BLOCK.
*/
/* ---- Optional header used for MCTEX files. */
/* If this is a MCTEX file, the first 4 words following the
mandatory header contain the string 'MCTEX'. */
memcpy(proj_name, (short *)(uf + uf_ma[2] - 1), 8);
if (little_endian()) swap2(proj_name, 4);
/* ---- Local Use Header (if present) was checked during Mandatory
* Header processing above.
*/
/* ---- Begining of FIELD HEADER. */
uf_fh = uf+current_fh_index - 1;
scale_factor = uf_fh[1];
ray->h.range_bin1 = uf_fh[2] * 1000.0 + uf_fh[3];
ray->h.gate_size = uf_fh[4];
ray->h.nbins = uf_fh[5];
ray->h.pulse_width = uf_fh[6]/(RSL_SPEED_OF_LIGHT/1.0e6);
if (strncmp((char *)proj_name, "MCTEX", 5) == 0) /* MCTEX? */
{
/* The beamwidth values are not correct in Mctex UF files. */
ray->h.beam_width = 1.0;
sweep->h.beam_width = ray->h.beam_width;
sweep->h.horz_half_bw = ray->h.beam_width/2.0;
sweep->h.vert_half_bw = ray->h.beam_width/2.0;
}
else /* Not MCTEX */
{
ray->h.beam_width = uf_fh[7] / 64.0;
sweep->h.beam_width = uf_fh[7] / 64.0;
sweep->h.horz_half_bw = uf_fh[7] / 128.0; /* DFF 4/4/95 */
sweep->h.vert_half_bw = uf_fh[8] / 128.0; /* DFF 4/4/95 */
}
/* fprintf (stderr, "uf_fh[7] = %d, [8] = %d\n", (int)uf_fh[7], (int)uf_fh[8]); */
if((int)uf_fh[7] == -32768) {
ray->h.beam_width = 1;
sweep->h.beam_width = 1;
sweep->h.horz_half_bw = .5;
sweep->h.vert_half_bw = .5;
}
ray->h.frequency = uf_fh[9] / 64.0;
ray->h.wavelength = uf_fh[11] / 64.0 / 100.0; /* cm to m. */
ray->h.pulse_count = uf_fh[12];
if (ifield == DZ_INDEX || ifield == ZT_INDEX) {
radar->v[ifield]->h.calibr_const = uf_fh[16] / 100.0;
/* uf value scaled by 100 */
}
else {
radar->v[ifield]->h.calibr_const = 0.0;
}
if (uf_fh[17] == (short)UF_NO_DATA) x = 0;
else x = uf_fh[17] / 1000000.0; /* PRT in seconds. */
if (x != 0) {
ray->h.prf = 1/x;
ray->h.unam_rng = RSL_SPEED_OF_LIGHT / (2.0 * ray->h.prf * 1000.0);
}
else {
ray->h.prf = 0.0;
ray->h.unam_rng = 0.0;
}
if (VR_INDEX == ifield || VE_INDEX == ifield) {
ray->h.nyq_vel = uf_fh[19] / scale_factor;
}
/* ---- End of FIELD HEADER. */
ray->h.f = f;
ray->h.invf = invf;
/* ---- Begining of FIELD DATA. */
uf_data = uf+uf_fh[0] - 1;
len_data = ray->h.nbins; /* Known because of RSL_new_ray. */
for (m=0; m<len_data; m++) {
if (uf_data[m] == (short)UF_NO_DATA)
ray->range[m] = invf(BADVAL); /* BADVAL */
else {
if(uf_data[m] == missing_data)
ray->range[m] = invf(NOECHO); /* NOECHO */
else
ray->range[m] = invf((float)uf_data[m]/scale_factor);
}
}
}
}
return UF_MORE;
}
void KFKSDS_steady (int *dim, double *sy, double *sZ, double *sT, double *sH,
double *sR, double *sV, double *sQ, double *sa0, double *sP0,
double *tol, int *maxiter, double *ksconvfactor,
double *mll, double *epshat, double *vareps,
double *etahat, double *vareta,
double *sumepsmisc, double *sumetamisc)
{
int i, ip1, n = dim[0], m = dim[2], ir = dim[3], convref, nmconvref, nm1 = n-1;
int irsod = ir * sizeof(double);
//double v[n], f[n], invf[n], vof[n];
std::vector<double> v(n), f(n), invf(n), vof(n);
sumepsmisc[0] = 0.0;
gsl_vector * sum_eta_misc = gsl_vector_calloc(ir);
gsl_vector * etahat_sq = gsl_vector_alloc(ir);
gsl_vector_view Z = gsl_vector_view_array(sZ, m);
gsl_vector * Z_cp = gsl_vector_alloc(m);
gsl_matrix * K = gsl_matrix_alloc(n, m);
gsl_vector_view K_irow;
gsl_matrix_view Q = gsl_matrix_view_array(sQ, m, m);
gsl_matrix_view V = gsl_matrix_view_array(sV, ir, ir);
gsl_matrix_view R = gsl_matrix_view_array(sR, m, ir);
gsl_matrix * r = gsl_matrix_alloc(n + 1, m);
gsl_vector_view r_row_t;
gsl_vector_view r_row_tp1 = gsl_matrix_row(r, n);
gsl_vector_set_zero(&r_row_tp1.vector);
std::vector<gsl_matrix*> L(n);
std::vector<gsl_matrix*> N(n+1);
N.at(n) = gsl_matrix_calloc(m, m);
gsl_vector_view Ndiag;
gsl_vector_view Qdiag = gsl_matrix_diagonal(&Q.matrix);
gsl_vector * Qdiag_msq = gsl_vector_alloc(m);
gsl_vector_memcpy(Qdiag_msq, &Qdiag.vector);
gsl_vector_mul(Qdiag_msq, &Qdiag.vector);
gsl_vector_scale(Qdiag_msq, -1.0);
gsl_vector * sum_vareta = gsl_vector_calloc(m);
KF_steady(dim, sy, sZ, sT, sH,
sR, sV, sQ, sa0, sP0,
mll, &v, &f, &invf, &vof, K, &L, tol, maxiter);
convref = dim[5];
if (convref == -1) {
convref = n;
} else
convref = ceil(convref * ksconvfactor[0]);
nmconvref = n - convref;
gsl_vector_view vaux;
gsl_matrix * Mmm = gsl_matrix_alloc(m, m);
gsl_matrix * ZtZ = gsl_matrix_alloc(m, m);
gsl_matrix_view maux1, maux2;
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&Z.vector, 0), m, 1);
gsl_vector_memcpy(Z_cp, &Z.vector);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix,
&maux2.matrix, 0.0, ZtZ);
gsl_vector * var_eps = gsl_vector_alloc(n);
double msHsq = -1.0 * pow(*sH, 2);
vaux = gsl_vector_view_array(&f[0], n);
gsl_vector_set_all(var_eps, msHsq);
gsl_vector_div(var_eps, &vaux.vector);
gsl_vector_add_constant(var_eps, *sH);
gsl_matrix * eta_hat = gsl_matrix_alloc(n, ir);
gsl_matrix * Mrm = gsl_matrix_alloc(ir, m);
gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &V.matrix, &R.matrix, 0.0, Mrm);
for (i = n-1; i > -1; i--)
{
ip1 = i + 1;
if (i != n-1) //the case i=n-1 was initialized above
r_row_tp1 = gsl_matrix_row(r, ip1);
r_row_t = gsl_matrix_row(r, i);
gsl_blas_dgemv(CblasTrans, 1.0, L.at(i), &r_row_tp1.vector,
0.0, &r_row_t.vector);
gsl_vector_memcpy(Z_cp, &Z.vector);
gsl_vector_scale(Z_cp, vof[i]);
gsl_vector_add(&r_row_t.vector, Z_cp);
N.at(i) = gsl_matrix_alloc(m, m);
if (i < convref || i > nmconvref)
{
gsl_matrix_memcpy(N.at(i), ZtZ);
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, L.at(i), N.at(ip1), 0.0, Mmm);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Mmm, L.at(i), invf[i], N.at(i));
} else {
gsl_matrix_memcpy(N.at(i), N.at(ip1));
}
if (dim[6] == 0 || dim[6] == 1)
{
if (i < convref || i == nm1) {
K_irow = gsl_matrix_row(K, i);
}
gsl_blas_ddot(&K_irow.vector, &r_row_tp1.vector, &epshat[i]);
epshat[i] -= vof[i];
epshat[i] *= -*sH;
if (i < convref || i > nmconvref)
{
maux1 = gsl_matrix_view_array(gsl_vector_ptr(&K_irow.vector, 0), 1, m);
maux2 = gsl_matrix_view_array(gsl_vector_ptr(Z_cp, 0), 1, m);
gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &maux1.matrix, N.at(ip1),
0.0, &maux2.matrix);
vaux = gsl_vector_view_array(gsl_vector_ptr(var_eps, i), 1);
gsl_blas_dgemv(CblasNoTrans, msHsq, &maux2.matrix, &K_irow.vector,
1.0, &vaux.vector);
vareps[i] = gsl_vector_get(&vaux.vector, 0);
} else {
vareps[i] = vareps[ip1];
}
sumepsmisc[0] += epshat[i] * epshat[i] + vareps[i];
}
if (dim[6] == 0 || dim[6] == 2)
{
vaux = gsl_matrix_row(eta_hat, i);
gsl_blas_dgemv(CblasNoTrans, 1.0, Mrm, &r_row_tp1.vector,
0.0, &vaux.vector);
memcpy(&etahat[i*ir], (&vaux.vector)->data, irsod);
if (i != n-1)
{
gsl_vector_memcpy(etahat_sq, &vaux.vector);
gsl_vector_mul(etahat_sq, etahat_sq);
gsl_vector_add(sum_eta_misc, etahat_sq);
}
if (i != n-1)
{
if (i < convref || i > nmconvref)
{
Ndiag = gsl_matrix_diagonal(N.at(ip1));
gsl_vector_memcpy(Z_cp, &Ndiag.vector);
gsl_vector_mul(Z_cp, Qdiag_msq);
gsl_vector_add(Z_cp, &Qdiag.vector);
gsl_vector_set_zero(sum_vareta);
gsl_vector_add(sum_vareta, Z_cp);
}
gsl_blas_dgemv(CblasTrans, 1.0, &R.matrix, sum_vareta, 1.0, sum_eta_misc);
}
}
gsl_matrix_free(L.at(i));
gsl_matrix_free(N.at(ip1));
}
gsl_matrix_free(N.at(0));
if (dim[6] == 0 || dim[6] == 2)
{
memcpy(&sumetamisc[0], sum_eta_misc->data, irsod);
}
gsl_vector_free(Z_cp);
gsl_vector_free(var_eps);
gsl_vector_free(Qdiag_msq);
gsl_vector_free(sum_vareta);
gsl_vector_free(sum_eta_misc);
gsl_vector_free(etahat_sq);
gsl_matrix_free(eta_hat);
gsl_matrix_free(Mrm);
gsl_matrix_free(r);
gsl_matrix_free(K);
gsl_matrix_free(ZtZ);
gsl_matrix_free(Mmm);
}
