//--------------------------------------------------------------------------
void Omu_IntODE::resize()
{
if (_dxt->dim == _nx + _nd && _ut->dim == _nu && _u->dim == _nd + _nu)
return;
int neq = _n * (1 + _nx + _nu);
v_resize(_y, neq);
v_resize(_u, _nd + _nu);
//
// variables for ADOL-C
//
v_resize(_x, _nd + 2 * _n + _nu);
v_resize(_v, _nd + 2 * _n + _nu);
m_resize(_X2, _nd + 2 * _n + _nu, _nx + _nu);
m_resize(_Y2, _n, _nx + _nu);
//
// variables for low level _sys->continuous callback
//
v_resize(_ut, _nu);
_dxt.resize(_nd + _n, _nx, _nu);
m_resize(_Yx, _nd + _n, _nx);
m_resize(_Yu, _nd + _n, _nu);
}
static void test_mgcr(ITER *ip, int i, MAT *Q, MAT *R)
#endif
{
VEC vt, vt1;
static MAT *R1 = MNULL;
static VEC *r = VNULL, *r1 = VNULL;
VEC *rr;
int k, j;
Real sm;
/* check Q*Q^T = I */
vt.dim = vt.max_dim = ip->b->dim;
vt1.dim = vt1.max_dim = ip->b->dim;
Q = m_resize(Q, i + 1, ip->b->dim);
R1 = m_resize(R1, i + 1, i + 1);
r = v_resize(r, ip->b->dim);
r1 = v_resize(r1, ip->b->dim);
MEM_STAT_REG(R1, TYPE_MAT);
MEM_STAT_REG(r, TYPE_VEC);
MEM_STAT_REG(r1, TYPE_VEC);
m_zero(R1);
for (k = 1; k <= i; k++)
for (j = 1; j <= i; j++) {
vt.ve = Q->me[k];
vt1.ve = Q->me[j];
R1->me[k][j] = in_prod(&vt, &vt1);
}
for (j = 1; j <= i; j++)
R1->me[j][j] -= 1.0;
#ifndef MEX
if (m_norm_inf(R1) > MACHEPS * ip->b->dim)
printf(" ! (mgcr:) m_norm_inf(Q*Q^T) = %g\n", m_norm_inf(R1));
#endif
/* check (r_i,Ap_j) = 0 for j <= i */
ip->Ax(ip->A_par, ip->x, r);
v_sub(ip->b, r, r);
rr = r;
if (ip->Bx) {
ip->Bx(ip->B_par, r, r1);
rr = r1;
}
#ifndef MEX
printf(" ||r|| = %g\n", v_norm2(rr));
#endif
sm = 0.0;
for (j = 1; j <= i; j++) {
vt.ve = Q->me[j];
sm = max(sm, in_prod(&vt,rr));
}
#ifndef MEX
if (sm >= MACHEPS * ip->b->dim)
printf(" ! (mgcr:) max_j (r,Ap_j) = %g\n", sm);
#endif
}
VEC *QRsolve(const MAT *QR, const VEC *diag, const VEC *b, VEC *x)
#endif
{
int limit;
STATIC VEC *tmp = VNULL;
if ( ! QR || ! diag || ! b )
error(E_NULL,"QRsolve");
limit = min(QR->m,QR->n);
if ( diag->dim < limit || b->dim != QR->m )
error(E_SIZES,"QRsolve");
tmp = v_resize(tmp,limit);
MEM_STAT_REG(tmp,TYPE_VEC);
x = v_resize(x,QR->n);
_Qsolve(QR,diag,b,x,tmp);
x = Usolve(QR,x,x,0.0);
v_resize(x,QR->n);
#ifdef THREADSAFE
V_FREE(tmp);
#endif
return x;
}
MAT *bifactor(MAT *A, MAT *U, MAT *V)
#endif
{
int k;
STATIC VEC *tmp1=VNULL, *tmp2=VNULL, *w=VNULL;
Real beta;
if ( ! A )
error(E_NULL,"bifactor");
if ( ( U && ( U->m != U->n ) ) || ( V && ( V->m != V->n ) ) )
error(E_SQUARE,"bifactor");
if ( ( U && U->m != A->m ) || ( V && V->m != A->n ) )
error(E_SIZES,"bifactor");
tmp1 = v_resize(tmp1,A->m);
tmp2 = v_resize(tmp2,A->n);
w = v_resize(w, max(A->m,A->n));
MEM_STAT_REG(tmp1,TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(w, TYPE_VEC);
if ( A->m >= A->n )
for ( k = 0; k < A->n; k++ )
{
get_col(A,k,tmp1);
hhvec(tmp1,k,&beta,tmp1,&(A->me[k][k]));
_hhtrcols(A,k,k+1,tmp1,beta,w);
if ( U )
_hhtrcols(U,k,0,tmp1,beta,w);
if ( k+1 >= A->n )
continue;
get_row(A,k,tmp2);
hhvec(tmp2,k+1,&beta,tmp2,&(A->me[k][k+1]));
hhtrrows(A,k+1,k+1,tmp2,beta);
if ( V )
_hhtrcols(V,k+1,0,tmp2,beta,w);
}
else
for ( k = 0; k < A->m; k++ )
{
get_row(A,k,tmp2);
hhvec(tmp2,k,&beta,tmp2,&(A->me[k][k]));
hhtrrows(A,k+1,k,tmp2,beta);
if ( V )
_hhtrcols(V,k,0,tmp2,beta,w);
if ( k+1 >= A->m )
continue;
get_col(A,k,tmp1);
hhvec(tmp1,k+1,&beta,tmp1,&(A->me[k+1][k]));
_hhtrcols(A,k+1,k+1,tmp1,beta,w);
if ( U )
_hhtrcols(U,k+1,0,tmp1,beta,w);
}
#ifdef THREADSAFE
V_FREE(tmp1); V_FREE(tmp2);
#endif
return A;
}
VEC *svd(MAT *A, MAT *U, MAT *V, VEC *d)
#endif
{
STATIC VEC *f=VNULL;
int i, limit;
MAT *A_tmp;
if ( ! A )
error(E_NULL,"svd");
if ( ( U && ( U->m != U->n ) ) || ( V && ( V->m != V->n ) ) )
error(E_SQUARE,"svd");
if ( ( U && U->m != A->m ) || ( V && V->m != A->n ) )
error(E_SIZES,"svd");
A_tmp = m_copy(A,MNULL);
if ( U != MNULL )
m_ident(U);
if ( V != MNULL )
m_ident(V);
limit = min(A_tmp->m,A_tmp->n);
d = v_resize(d,limit);
f = v_resize(f,limit-1);
MEM_STAT_REG(f,TYPE_VEC);
bifactor(A_tmp,U,V);
if ( A_tmp->m >= A_tmp->n )
for ( i = 0; i < limit; i++ )
{
d->ve[i] = A_tmp->me[i][i];
if ( i+1 < limit )
f->ve[i] = A_tmp->me[i][i+1];
}
else
for ( i = 0; i < limit; i++ )
{
d->ve[i] = A_tmp->me[i][i];
if ( i+1 < limit )
f->ve[i] = A_tmp->me[i+1][i];
}
if ( A_tmp->m >= A_tmp->n )
bisvd(d,f,U,V);
else
bisvd(d,f,V,U);
M_FREE(A_tmp);
#ifdef THREADSAFE
V_FREE(f);
#endif
return d;
}
MAT *makeQ(const MAT *QR,const VEC *diag, MAT *Qout)
#endif
{
STATIC VEC *tmp1=VNULL,*tmp2=VNULL;
unsigned int i, limit;
Real beta, r_ii, tmp_val;
int j;
limit = min(QR->m,QR->n);
if ( ! QR || ! diag )
error(E_NULL,"makeQ");
if ( diag->dim < limit )
error(E_SIZES,"makeQ");
if ( Qout==(MAT *)NULL || Qout->m < QR->m || Qout->n < QR->m )
Qout = m_get(QR->m,QR->m);
tmp1 = v_resize(tmp1,QR->m); /* contains basis vec & columns of Q */
tmp2 = v_resize(tmp2,QR->m); /* contains H/holder vectors */
MEM_STAT_REG(tmp1,TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
for ( i=0; i<QR->m ; i++ )
{ /* get i-th column of Q */
/* set up tmp1 as i-th basis vector */
for ( j=0; j<QR->m ; j++ )
tmp1->ve[j] = 0.0;
tmp1->ve[i] = 1.0;
/* apply H/h transforms in reverse order */
for ( j=limit-1; j>=0; j-- )
{
get_col(QR,j,tmp2);
r_ii = fabs(tmp2->ve[j]);
tmp2->ve[j] = diag->ve[j];
tmp_val = (r_ii*fabs(diag->ve[j]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
/* hhtrvec(tmp2,beta->ve[j],j,tmp1,tmp1); */
hhtrvec(tmp2,beta,j,tmp1,tmp1);
}
/* insert into Q */
set_col(Qout,i,tmp1);
}
#ifdef THREADSAFE
V_FREE(tmp1); V_FREE(tmp2);
#endif
return (Qout);
}
static MAT *calc_VinvIminAw(MAT *Vw, MAT *X, MAT *VinvIminAw, int calc_Aw) {
/*
* calculate V_w^-1(I-A_w) (==VinvIminAw),
* A = X(X'X)^-1 X' (AY = XBeta; Beta = (X'X)^-1 X'Y)
*
* on second thought (Nov 1998 -- more than 4 years later :-))
* calc (I-Aw) only once and keep this constant during iteration.
*/
MAT *tmp = MNULL, *V = MNULL;
VEC *b = VNULL, *rhs = VNULL;
int i, j;
if (X->m != Vw->n || VinvIminAw->m != X->m)
ErrMsg(ER_IMPOSVAL, "calc_VinvIminAw: sizes don't match");
if (calc_Aw) {
IminAw = m_resize(IminAw, X->m, X->m);
tmp = m_resize(tmp, X->n, X->n);
tmp = mtrm_mlt(X, X, tmp); /* X'X */
m_inverse(tmp, tmp); /* (X'X)-1 */
/* X(X'X)-1 -> X(X'X)-1 X') */
IminAw = XVXt_mlt(X, tmp, IminAw);
for (i = 0; i < IminAw->m; i++) /* I - Aw */
for (j = 0; j <= i; j++)
if (i == j)
IminAw->me[i][j] = 1.0 - IminAw->me[i][j];
else
IminAw->me[i][j] = IminAw->me[j][i] = -IminAw->me[i][j];
}
V = m_copy(Vw, V);
LDLfactor(V);
rhs = v_resize(rhs, X->m);
b = v_resize(b, X->m);
for (i = 0; i < X->m; i++) { /* solve Vw X = (I-A) for X -> V-1(I-A) */
rhs = get_col(IminAw, i, rhs);
LDLsolve(V, rhs, b);
set_col(VinvIminAw, i, b);
}
v_free(rhs);
v_free(b);
m_free(V);
if (tmp)
m_free(tmp);
return VinvIminAw;
}
MAT *makeHQ(MAT *H, VEC *diag, VEC *beta, MAT *Qout)
#endif
{
int i, j, limit;
STATIC VEC *tmp1 = VNULL, *tmp2 = VNULL;
if ( H==(MAT *)NULL || diag==(VEC *)NULL || beta==(VEC *)NULL )
error(E_NULL,"makeHQ");
limit = H->m - 1;
if ( diag->dim < limit || beta->dim < limit )
error(E_SIZES,"makeHQ");
if ( H->m != H->n )
error(E_SQUARE,"makeHQ");
Qout = m_resize(Qout,H->m,H->m);
tmp1 = v_resize(tmp1,H->m);
tmp2 = v_resize(tmp2,H->m);
MEM_STAT_REG(tmp1,TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
for ( i = 0; i < H->m; i++ )
{
/* tmp1 = i'th basis vector */
for ( j = 0; j < H->m; j++ )
/* tmp1->ve[j] = 0.0; */
v_set_val(tmp1,j,0.0);
/* tmp1->ve[i] = 1.0; */
v_set_val(tmp1,i,1.0);
/* apply H/h transforms in reverse order */
for ( j = limit-1; j >= 0; j-- )
{
get_col(H,(unsigned int)j,tmp2);
/* tmp2->ve[j+1] = diag->ve[j]; */
v_set_val(tmp2,j+1,v_entry(diag,j));
hhtrvec(tmp2,beta->ve[j],j+1,tmp1,tmp1);
}
/* insert into Qout */
set_col(Qout,(unsigned int)i,tmp1);
}
#ifdef THREADSAFE
V_FREE(tmp1); V_FREE(tmp2);
#endif
return (Qout);
}
void booz_sensors_model_accel_run( double time ) {
if (time < bsm.accel_next_update)
return;
static VEC* accel_ltp = VNULL;
accel_ltp = v_resize(accel_ltp, AXIS_NB);
/* substract gravity to acceleration in ltp frame */
accel_ltp = v_sub(bfm.accel_ltp, bfm.g_ltp, accel_ltp);
/* convert to body frame */
static VEC* accel_body = VNULL;
accel_body = v_resize(accel_body, AXIS_NB);
mv_mlt(bfm.dcm, accel_ltp, accel_body);
/* convert to imu frame */
static VEC* accel_imu = VNULL;
accel_imu = v_resize(accel_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, accel_body, accel_imu);
// printf(" accel_body ~ %f %f %f\n", accel_body->ve[AXIS_X], accel_body->ve[AXIS_Y], accel_body->ve[AXIS_Z]);
/* compute accel reading */
mv_mlt(bsm.accel_sensitivity, accel_imu, bsm.accel);
v_add(bsm.accel, bsm.accel_neutral, bsm.accel);
/* compute accel error readings */
static VEC *accel_error = VNULL;
accel_error = v_resize(accel_error, AXIS_NB);
accel_error = v_zero(accel_error);
/* add a gaussian noise */
accel_error = v_add_gaussian_noise(accel_error, bsm.accel_noise_std_dev, accel_error);
/* constant bias */
accel_error = v_add(accel_error, bsm.accel_bias, accel_error);
/* scale to adc units FIXME : should use full adc gain ? sum ? */
accel_error->ve[AXIS_X] = accel_error->ve[AXIS_X] * bsm.accel_sensitivity->me[AXIS_X][AXIS_X];
accel_error->ve[AXIS_Y] = accel_error->ve[AXIS_Y] * bsm.accel_sensitivity->me[AXIS_Y][AXIS_Y];
accel_error->ve[AXIS_Z] = accel_error->ve[AXIS_Z] * bsm.accel_sensitivity->me[AXIS_Z][AXIS_Z];
/* add per accel error reading */
bsm.accel = v_add(bsm.accel, accel_error, bsm.accel);
/* round signal to account for adc discretisation */
RoundSensor(bsm.accel);
/* saturation */
BoundSensor(bsm.accel, 0, bsm.accel_resolution);
// printf("sim adc %f %f %f\n",bsm.accel->ve[AXIS_X] ,bsm.accel->ve[AXIS_Y] ,bsm.accel->ve[AXIS_Z]);
bsm.accel_next_update += BSM_ACCEL_DT;
bsm.accel_available = TRUE;
}
VEC *pxinv_vec(PERM *px, const VEC *x, VEC *out)
#endif
{
unsigned int i, size;
if ( ! px || ! x )
error(E_NULL,"pxinv_vec");
if ( px->size > x->dim )
error(E_SIZES,"pxinv_vec");
/* if ( x == out )
error(E_INSITU,"pxinv_vec"); */
if ( ! out || out->dim < x->dim )
out = v_resize(out,x->dim);
size = px->size;
if ( size == 0 )
return v_copy(x,out);
if ( out != x )
{
for ( i=0; i<size; i++ )
if ( px->pe[i] >= size )
error(E_BOUNDS,"pxinv_vec");
else
out->ve[px->pe[i]] = x->ve[i];
}
else
{ /* in situ algorithm --- cheat's way out */
px_inv(px,px);
px_vec(px,x,out);
px_inv(px,px);
}
return out;
}
VEC *vm_mlt(const MAT *A, const VEC *b, VEC *out)
#endif
{
unsigned int j,m,n;
/* Real sum,**A_v,*b_v; */
if ( A==(MAT *)NULL || b==(VEC *)NULL )
error(E_NULL,"vm_mlt");
if ( A->m != b->dim )
error(E_SIZES,"vm_mlt");
if ( b == out )
error(E_INSITU,"vm_mlt");
if ( out == (VEC *)NULL || out->dim != A->n )
out = v_resize(out,A->n);
m = A->m; n = A->n;
v_zero(out);
for ( j = 0; j < m; j++ )
if ( b->ve[j] != 0.0 )
__mltadd__(out->ve,A->me[j],b->ve[j],(int)n);
/**************************************************
A_v = A->me; b_v = b->ve;
for ( j=0; j<n; j++ )
{
sum = 0.0;
for ( i=0; i<m; i++ )
sum += b_v[i]*A_v[i][j];
out->ve[j] = sum;
}
**************************************************/
return out;
}
VEC *pxinv_vec(PERM *px, const VEC *x, VEC *out)
{
unsigned int i, size;
if ( ! px || ! x )
error(E_NULL,"pxinv_vec");
if ( px->size > x->dim )
error(E_SIZES,"pxinv_vec");
if ( ! out || out->dim < x->dim )
out = v_resize(out,x->dim);
size = px->size;
if ( size == 0 )
return v_copy(x,out);
if ( out != x )
{
for ( i=0; i<size; i++ )
if ( px->pe[i] >= size )
error(E_BOUNDS,"pxinv_vec");
else
out->ve[px->pe[i]] = x->ve[i];
}
else
{
px_inv(px,px);
px_vec(px,x,out);
px_inv(px,px);
}
return out;
}
/* hhtrcols -- transform a matrix by a Householder vector by columns
starting at row i0 from column j0 -- in-situ */
MAT *hhtrcols(MAT *M,unsigned int i0,unsigned int j0,VEC *hh,double beta)
{
/* Real ip, scale; */
int i /*, k */;
static VEC *w = VNULL;
if ( M==(MAT *)NULL || hh==(VEC *)NULL )
error(E_NULL,"hhtrcols");
if ( M->m != hh->dim )
error(E_SIZES,"hhtrcols");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"hhtrcols");
if ( beta == 0.0 ) return (M);
w = v_resize(w,M->n);
MEM_STAT_REG(w,TYPE_VEC);
v_zero(w);
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],
(int)(M->n-j0));
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],
(int)(M->n-j0));
return (M);
}
VEC *mv_mlt(const MAT *A, const VEC *b, VEC *out)
#endif
{
unsigned int i, m, n;
Real **A_v, *b_v /*, *A_row */;
/* register Real sum; */
if ( A==(MAT *)NULL || b==(VEC *)NULL )
error(E_NULL,"mv_mlt");
if ( A->n != b->dim )
error(E_SIZES,"mv_mlt");
if ( b == out )
error(E_INSITU,"mv_mlt");
if ( out == (VEC *)NULL || out->dim != A->m )
out = v_resize(out,A->m);
m = A->m; n = A->n;
A_v = A->me; b_v = b->ve;
for ( i=0; i<m; i++ )
{
/* for ( j=0; j<n; j++ )
sum += A_v[i][j]*b_v[j]; */
out->ve[i] = __ip__(A_v[i],b_v,(int)n);
/**************************************************
A_row = A_v[i]; b_v = b->ve;
for ( j=0; j<n; j++ )
sum += (*A_row++)*(*b_v++);
out->ve[i] = sum;
**************************************************/
}
return out;
}
MAT *hhtrcols(MAT *M, unsigned int i0, unsigned int j0,
const VEC *hh, double beta)
#endif
{
STATIC VEC *w = VNULL;
if ( M == MNULL || hh == VNULL || w == VNULL )
error(E_NULL,"hhtrcols");
if ( M->m != hh->dim )
error(E_SIZES,"hhtrcols");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"hhtrcols");
if ( ! w || w->dim < M->n )
w = v_resize(w,M->n);
MEM_STAT_REG(w,TYPE_VEC);
M = _hhtrcols(M,i0,j0,hh,beta,w);
#ifdef THREADSAFE
V_FREE(w);
#endif
return M;
}
MAT *_hhtrcols(MAT *M, unsigned int i0, unsigned int j0,
const VEC *hh, double beta, VEC *w)
#endif
{
/* Real ip, scale; */
int i /*, k */;
/* STATIC VEC *w = VNULL; */
if ( M == MNULL || hh == VNULL || w == VNULL )
error(E_NULL,"_hhtrcols");
if ( M->m != hh->dim )
error(E_SIZES,"_hhtrcols");
if ( i0 > M->m || j0 > M->n )
error(E_BOUNDS,"_hhtrcols");
if ( beta == 0.0 ) return (M);
if ( w->dim < M->n )
w = v_resize(w,M->n);
/* MEM_STAT_REG(w,TYPE_VEC); */
v_zero(w);
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(w->ve[j0]),&(M->me[i][j0]),hh->ve[i],
(int)(M->n-j0));
for ( i = i0; i < M->m; i++ )
if ( hh->ve[i] != 0.0 )
__mltadd__(&(M->me[i][j0]),&(w->ve[j0]),-beta*hh->ve[i],
(int)(M->n-j0));
return (M);
}
/* v_conv -- computes convolution product of two vectors */
VEC *v_conv(VEC *x1, VEC *x2, VEC *out)
{
int i;
if ( ! x1 || ! x2 )
error(E_NULL,"v_conv");
if ( x1 == out || x2 == out )
error(E_INSITU,"v_conv");
if ( x1->dim == 0 || x2->dim == 0 )
return out = v_resize(out,0);
out = v_resize(out,x1->dim + x2->dim - 1);
v_zero(out);
for ( i = 0; i < x1->dim; i++ )
__mltadd__(&(out->ve[i]),x2->ve,x1->ve[i],x2->dim);
return out;
}
MAT *Hfactor(MAT *A, VEC *diag, VEC *beta)
#endif
{
STATIC VEC *hh = VNULL, *w = VNULL;
int k, limit;
if ( ! A || ! diag || ! beta )
error(E_NULL,"Hfactor");
if ( diag->dim < A->m - 1 || beta->dim < A->m - 1 )
error(E_SIZES,"Hfactor");
if ( A->m != A->n )
error(E_SQUARE,"Hfactor");
limit = A->m - 1;
hh = v_resize(hh,A->m);
w = v_resize(w,A->n);
MEM_STAT_REG(hh,TYPE_VEC);
MEM_STAT_REG(w, TYPE_VEC);
for ( k = 0; k < limit; k++ )
{
/* compute the Householder vector hh */
get_col(A,(unsigned int)k,hh);
/* printf("the %d'th column = "); v_output(hh); */
hhvec(hh,k+1,&beta->ve[k],hh,&A->me[k+1][k]);
/* diag->ve[k] = hh->ve[k+1]; */
v_set_val(diag,k,v_entry(hh,k+1));
/* printf("H/h vector = "); v_output(hh); */
/* printf("from the %d'th entry\n",k+1); */
/* printf("beta = %g\n",beta->ve[k]); */
/* apply Householder operation symmetrically to A */
_hhtrcols(A,k+1,k+1,hh,v_entry(beta,k),w);
hhtrrows(A,0 ,k+1,hh,v_entry(beta,k));
/* printf("A = "); m_output(A); */
}
#ifdef THREADSAFE
V_FREE(hh); V_FREE(w);
#endif
return (A);
}
void booz_sensors_model_mag_run( double time ) {
if (time < bsm.mag_next_update)
return;
/* rotate h to body frame */
static VEC *h_body = VNULL;
h_body = v_resize(h_body, AXIS_NB);
mv_mlt(bfm.dcm, bfm.h_ltp, h_body);
/* rotate to imu frame */
static VEC *h_imu = VNULL;
h_imu = v_resize(h_imu, AXIS_NB);
mv_mlt(bsm.body_to_imu, h_body, h_imu);
/* rotate to sensor frame */
static VEC *h_sensor = VNULL;
h_sensor = v_resize(h_sensor, AXIS_NB);
mv_mlt(bsm.mag_imu_to_sensor, h_imu, h_sensor);
mv_mlt(bsm.mag_sensitivity, h_sensor, bsm.mag);
v_add(bsm.mag, bsm.mag_neutral, bsm.mag);
/* compute mag error readings */
static VEC *mag_error = VNULL;
mag_error = v_resize(mag_error, AXIS_NB);
/* add hard iron now ? */
mag_error = v_zero(mag_error);
/* add a gaussian noise */
mag_error = v_add_gaussian_noise(mag_error, bsm.mag_noise_std_dev, mag_error);
mag_error->ve[AXIS_X] = mag_error->ve[AXIS_X] * bsm.mag_sensitivity->me[AXIS_X][AXIS_X];
mag_error->ve[AXIS_Y] = mag_error->ve[AXIS_Y] * bsm.mag_sensitivity->me[AXIS_Y][AXIS_Y];
mag_error->ve[AXIS_Z] = mag_error->ve[AXIS_Z] * bsm.mag_sensitivity->me[AXIS_Z][AXIS_Z];
/* add error */
v_add(bsm.mag, mag_error, bsm.mag);
// printf("h body %f %f %f\n", h_body->ve[AXIS_X], h_body->ve[AXIS_Y], h_body->ve[AXIS_Z]);
// printf("mag %f %f %f\n", bsm.mag->ve[AXIS_X], bsm.mag->ve[AXIS_Y], bsm.mag->ve[AXIS_Z]);
/* round signal to account for adc discretisation */
RoundSensor(bsm.mag);
bsm.mag_next_update += BSM_MAG_DT;
bsm.mag_available = TRUE;
}
MAT *m_inverse(const MAT *A, MAT *out)
#endif
{
int i;
STATIC VEC *tmp = VNULL, *tmp2 = VNULL;
STATIC MAT *A_cp = MNULL;
STATIC PERM *pivot = PNULL;
if ( ! A )
error(E_NULL,"m_inverse");
if ( A->m != A->n )
error(E_SQUARE,"m_inverse");
if ( ! out || out->m < A->m || out->n < A->n )
out = m_resize(out,A->m,A->n);
A_cp = m_resize(A_cp,A->m,A->n);
A_cp = m_copy(A,A_cp);
tmp = v_resize(tmp,A->m);
tmp2 = v_resize(tmp2,A->m);
pivot = px_resize(pivot,A->m);
MEM_STAT_REG(A_cp,TYPE_MAT);
MEM_STAT_REG(tmp, TYPE_VEC);
MEM_STAT_REG(tmp2,TYPE_VEC);
MEM_STAT_REG(pivot,TYPE_PERM);
tracecatch(LUfactor(A_cp,pivot),"m_inverse");
for ( i = 0; i < A->n; i++ )
{
v_zero(tmp);
tmp->ve[i] = 1.0;
tracecatch(LUsolve(A_cp,pivot,tmp,tmp2),"m_inverse");
set_col(out,i,tmp2);
}
#ifdef THREADSAFE
V_FREE(tmp); V_FREE(tmp2);
M_FREE(A_cp); PX_FREE(pivot);
#endif
return out;
}
double LUcondest(const MAT *LU, PERM *pivot)
#endif
{
STATIC VEC *y = VNULL, *z = VNULL;
Real cond_est, L_norm, U_norm, sum, tiny;
int i, j, n;
if ( ! LU || ! pivot )
error(E_NULL,"LUcondest");
if ( LU->m != LU->n )
error(E_SQUARE,"LUcondest");
if ( LU->n != pivot->size )
error(E_SIZES,"LUcondest");
tiny = 10.0/HUGE_VAL;
n = LU->n;
y = v_resize(y,n);
z = v_resize(z,n);
MEM_STAT_REG(y,TYPE_VEC);
MEM_STAT_REG(z,TYPE_VEC);
for ( i = 0; i < n; i++ )
{
sum = 0.0;
for ( j = 0; j < i; j++ )
sum -= LU->me[j][i]*y->ve[j];
sum -= (sum < 0.0) ? 1.0 : -1.0;
if ( fabs(LU->me[i][i]) <= tiny*fabs(sum) )
return HUGE_VAL;
y->ve[i] = sum / LU->me[i][i];
}
Catch(E_SING,
LTsolve(LU,y,y,1.0);
LUsolve(LU,pivot,y,z);
,
return HUGE_VAL);
VEC *LTsolve(const MAT *L, const VEC *b, VEC *out, double diag)
{
unsigned int dim;
int i, i_lim;
MatrixReal **L_me, *b_ve, *out_ve, tmp, invdiag, tiny;
if ( ! L || ! b )
error(E_NULL,"LTsolve");
dim = mat_min(L->m,L->n);
if ( b->dim < dim )
error(E_SIZES,"LTsolve");
out = v_resize(out,L->n);
L_me = L->me; b_ve = b->ve; out_ve = out->ve;
tiny = (10.0/HUGE_VAL);
for ( i=dim-1; i>=0; i-- )
if ( b_ve[i] != 0.0 )
break;
i_lim = i;
if ( b != out )
{
__zero__(out_ve,out->dim);
MEM_COPY(b_ve,out_ve,(i_lim+1)*sizeof(MatrixReal));
}
if ( diag == 0.0 )
{
for ( ; i>=0; i-- )
{
tmp = L_me[i][i];
if ( fabs(tmp) <= tiny*fabs(out_ve[i]) )
error(E_SING,"LTsolve");
out_ve[i] /= tmp;
__mltadd__(out_ve,L_me[i],-out_ve[i],i);
}
}
else
{
invdiag = 1.0/diag;
for ( ; i>=0; i-- )
{
out_ve[i] *= invdiag;
__mltadd__(out_ve,L_me[i],-out_ve[i],i);
}
}
return (out);
}
VEC *_Qsolve(const MAT *QR, const VEC *diag, const VEC *b,
VEC *x, VEC *tmp)
#endif
{
unsigned int dynamic;
int k, limit;
Real beta, r_ii, tmp_val;
limit = min(QR->m,QR->n);
dynamic = FALSE;
if ( ! QR || ! diag || ! b )
error(E_NULL,"_Qsolve");
if ( diag->dim < limit || b->dim != QR->m )
error(E_SIZES,"_Qsolve");
x = v_resize(x,QR->m);
if ( tmp == VNULL )
dynamic = TRUE;
tmp = v_resize(tmp,QR->m);
/* apply H/holder transforms in normal order */
x = v_copy(b,x);
for ( k = 0 ; k < limit ; k++ )
{
get_col(QR,k,tmp);
r_ii = fabs(tmp->ve[k]);
tmp->ve[k] = diag->ve[k];
tmp_val = (r_ii*fabs(diag->ve[k]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
/* hhtrvec(tmp,beta->ve[k],k,x,x); */
hhtrvec(tmp,beta,k,x,x);
}
if ( dynamic )
V_FREE(tmp);
return (x);
}
MAT *QRfactor(MAT *A, VEC *diag)
#endif
{
unsigned int k,limit;
Real beta;
STATIC VEC *hh=VNULL, *w=VNULL;
if ( ! A || ! diag )
error(E_NULL,"QRfactor");
limit = min(A->m,A->n);
if ( diag->dim < limit )
error(E_SIZES,"QRfactor");
hh = v_resize(hh,A->m);
w = v_resize(w, A->n);
MEM_STAT_REG(hh,TYPE_VEC);
MEM_STAT_REG(w, TYPE_VEC);
for ( k=0; k<limit; k++ )
{
/* get H/holder vector for the k-th column */
get_col(A,k,hh);
/* hhvec(hh,k,&beta->ve[k],hh,&A->me[k][k]); */
hhvec(hh,k,&beta,hh,&A->me[k][k]);
diag->ve[k] = hh->ve[k];
/* apply H/holder vector to remaining columns */
/* hhtrcols(A,k,k+1,hh,beta->ve[k]); */
_hhtrcols(A,k,k+1,hh,beta,w);
}
#ifdef THREADSAFE
V_FREE(hh); V_FREE(w);
#endif
return (A);
}
VEC *QRTsolve(const MAT *A, const VEC *diag, const VEC *c, VEC *sc)
#endif
{
int i, j, k, n, p;
Real beta, r_ii, s, tmp_val;
if ( ! A || ! diag || ! c )
error(E_NULL,"QRTsolve");
if ( diag->dim < min(A->m,A->n) )
error(E_SIZES,"QRTsolve");
sc = v_resize(sc,A->m);
n = sc->dim;
p = c->dim;
if ( n == p )
k = p-2;
else
k = p-1;
v_zero(sc);
sc->ve[0] = c->ve[0]/A->me[0][0];
if ( n == 1)
return sc;
if ( p > 1)
{
for ( i = 1; i < p; i++ )
{
s = 0.0;
for ( j = 0; j < i; j++ )
s += A->me[j][i]*sc->ve[j];
if ( A->me[i][i] == 0.0 )
error(E_SING,"QRTsolve");
sc->ve[i]=(c->ve[i]-s)/A->me[i][i];
}
}
for (i = k; i >= 0; i--)
{
s = diag->ve[i]*sc->ve[i];
for ( j = i+1; j < n; j++ )
s += A->me[j][i]*sc->ve[j];
r_ii = fabs(A->me[i][i]);
tmp_val = (r_ii*fabs(diag->ve[i]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
tmp_val = beta*s;
sc->ve[i] -= tmp_val*diag->ve[i];
for ( j = i+1; j < n; j++ )
sc->ve[j] -= tmp_val*A->me[j][i];
}
return sc;
}
VEC *bdLUsolve(const BAND *bA, PERM *pivot, const VEC *b, VEC *x)
#endif
{
int i,j,l,n,n1,pi,lb,ub,jmin, maxj;
Real c;
Real **bA_v;
if ( bA==(BAND *)NULL || b==(VEC *)NULL || pivot==(PERM *)NULL )
error(E_NULL,"bdLUsolve");
if ( bA->mat->n != b->dim || bA->mat->n != pivot->size)
error(E_SIZES,"bdLUsolve");
lb = bA->lb;
ub = bA->ub;
n = b->dim;
n1 = n-1;
bA_v = bA->mat->me;
x = v_resize(x,b->dim);
px_vec(pivot,b,x);
/* solve Lx = b; implicit diagonal = 1
L is not permuted, therefore it must be permuted now
*/
px_inv(pivot,pivot);
for (j=0; j < n; j++) {
jmin = j+1;
c = x->ve[j];
maxj = max(0,j+lb-n1);
for (i=jmin,l=lb-1; l >= maxj; i++,l--) {
if ( (pi = pivot->pe[i]) < jmin)
pi = pivot->pe[i] = pivot->pe[pi];
x->ve[pi] -= bA_v[l][j]*c;
}
}
/* solve Ux = b; explicit diagonal */
x->ve[n1] /= bA_v[lb][n1];
for (i=n-2; i >= 0; i--) {
c = x->ve[i];
for (j=min(n1,i+ub), l=lb+j-i; j > i; j--,l--)
c -= bA_v[l][j]*x->ve[j];
x->ve[i] = c/bA_v[lb][i];
}
return (x);
}
VEC *UTsolve(const MAT *U, const VEC *b, VEC *out, double diag)
{
unsigned int dim, i, i_lim;
MatrixReal **U_me, *b_ve, *out_ve, tmp, invdiag, tiny;
if ( ! U || ! b )
error(E_NULL,"UTsolve");
dim = mat_min(U->m,U->n);
if ( b->dim < dim )
error(E_SIZES,"UTsolve");
out = v_resize(out,U->n);
U_me = U->me; b_ve = b->ve; out_ve = out->ve;
tiny = (10.0/HUGE_VAL);
for ( i=0; i<dim; i++ )
if ( b_ve[i] != 0.0 )
break;
else
out_ve[i] = 0.0;
i_lim = i;
if ( b != out )
{
__zero__(out_ve,out->dim);
MEM_COPY(&(b_ve[i_lim]),&(out_ve[i_lim]),(dim-i_lim)*sizeof(MatrixReal));
}
if ( diag == 0.0 )
{
for ( ; i<dim; i++ )
{
tmp = U_me[i][i];
if ( fabs(tmp) <= tiny*fabs(out_ve[i]) )
error(E_SING,"UTsolve");
out_ve[i] /= tmp;
__mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
}
}
else
{
invdiag = 1.0/diag;
for ( ; i<dim; i++ )
{
out_ve[i] *= invdiag;
__mltadd__(&(out_ve[i+1]),&(U_me[i][i+1]),-out_ve[i],dim-i-1);
}
}
return (out);
}
/* v_move -- copies selected pieces of a vector
-- moves the length dim0 subvector with initial index i0
to the corresponding subvector of out with initial index i1
-- out is resized if necessary */
VEC *v_move(VEC *in,int i0,int dim0,VEC *out,int i1)
{
if ( ! in )
error(E_NULL,"v_move");
if ( i0 < 0 || dim0 < 0 || i1 < 0 ||
i0+dim0 > in->dim )
error(E_BOUNDS,"v_move");
if ( (! out) || i1+dim0 > out->dim )
out = v_resize(out,i1+dim0);
MEM_COPY(&(in->ve[i0]),&(out->ve[i1]),dim0*sizeof(Real));
return out;
}
/* v_star -- computes componentwise (Hadamard) product of x1 and x2
-- result out is returned */
VEC *v_star(VEC *x1,VEC *x2,VEC *out)
{
int i;
if ( ! x1 || ! x2 )
error(E_NULL,"v_star");
if ( x1->dim != x2->dim )
error(E_SIZES,"v_star");
out = v_resize(out,x1->dim);
for ( i = 0; i < x1->dim; i++ )
out->ve[i] = x1->ve[i] * x2->ve[i];
return out;
}
extern int v_resize_vars(int new_dim,...)
{
va_list ap;
int i=0;
VEC **par;
va_start(ap, new_dim);
while (par = va_arg(ap,VEC **)) { /* NULL ends the list*/
*par = v_resize(*par,new_dim);
i++;
}
va_end(ap);
return i;
}