// kernelRegression
// x:data
// dimention: x's dimention
// y: label of x
// num: number of data
// result: coeffience
double *kernelRegression(double **x,int dimention,double *y,int num,double gamma,double regilarization){
VEC *label;
MAT *gram,*ident,*inv;
int i,j;
double *result;
ident = m_get(num,num);
label = v_get(num);
memcpy(label->ve,y,sizeof(double)*num);
gram = m_get(num,num);
ident = m_get(num,num);
for(i=0;i<num;i++){
for(j=0;j<num;j++){
gram->me[i][j]=kernel(x[i],x[j],dimention,gamma);
}
}
inv=m_add(gram,sm_mlt(regilarization,m_ident(ident),NULL),NULL);
inv=m_inverse(inv,NULL); //memory leak.
VEC *coefficient=v_get(num);
mv_mlt(inv,label,coefficient);
result=malloc(sizeof(double)*num);
memcpy(result,coefficient->ve,sizeof(double)*num);
m_free(ident);
m_free(gram);
m_free(inv);
v_free(label);
v_free(coefficient);
return result;
}
static void kalman_init(kalman_t *kf, float q, float r, float pos, float speed)
{
kf->t0 = v_get(2);
kf->t1 = v_get(2);
kf->T0 = m_get(2, 2);
kf->T1 = m_get(2, 2);
kf->I = m_get(2, 2);
m_ident(kf->I);
/* set initial state: */
kf->x = v_get(2);
v_set_val(kf->x, 0, pos);
v_set_val(kf->x, 1, speed);
/* no measurement or control yet: */
kf->z = v_get(2);
kf->u = v_get(1);
kf->P = m_get(2, 2);
m_ident(kf->P);
/* set up noise: */
kf->Q = m_get(2, 2);
sm_mlt(q, kf->I, kf->Q);
kf->R = m_get(2, 2);
sm_mlt(r, kf->I, kf->R);
kf->K = m_get(2, 1);
kf->H = m_get(2, 2);
m_set_val(kf->H, 0, 0, 1.0);
//m_ident(kf->H);
/* A = | 1.0 dt |
| 0.0 1.0 |
dt values is set in kalman_run */
kf->A = m_get(2, 2);
m_set_val(kf->A, 0, 0, 1.0);
m_set_val(kf->A, 1, 0, 0.0);
m_set_val(kf->A, 1, 1, 1.0);
/* B = | 0.5 * dt ^ 2 |
| dt |
dt values are set in kalman_run */
kf->B = m_get(2, 1);
}
BAND *sbd_mlt(Real s, const BAND *A, BAND *OUT)
#endif
{
if ( ! A )
error(E_NULL,"sbd_mlt");
OUT = bd_resize(OUT,A->lb,A->ub,A->mat->n);
sm_mlt(s,A->mat,OUT->mat);
return OUT;
}
static int reml(VEC *Y, MAT *X, MAT **Vk, int n_k, int max_iter,
double fit_limit, VEC *teta) {
volatile int n_iter = 0;
int i;
volatile double rel_step = DBL_MAX;
VEC *rhs = VNULL;
VEC *dteta = VNULL;
MAT *Vw = MNULL, *Tr_m = MNULL, *VinvIminAw = MNULL;
Vw = m_resize(Vw, X->m, X->m);
VinvIminAw = m_resize(VinvIminAw, X->m, X->m);
rhs = v_resize(rhs, n_k);
Tr_m = m_resize(Tr_m, n_k, n_k);
dteta = v_resize(dteta, n_k);
while (n_iter < max_iter && rel_step > fit_limit) {
print_progress(n_iter, max_iter);
n_iter++;
dteta = v_copy(teta, dteta);
/* fill Vw, calc VinvIminAw, rhs; */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, n_iter == 1);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
/* Tr_m * teta = Rhs; symmetric, solve for teta: */
LDLfactor(Tr_m);
LDLsolve(Tr_m, rhs, teta);
if (DEBUG_VGMFIT) {
printlog("teta_%d [", n_iter);
for (i = 0; i < teta->dim; i++)
printlog(" %g", teta->ve[i]);
printlog("] -(log.likelyhood): %g\n",
calc_ll(Vw, X, Y, n_k));
}
v_sub(teta, dteta, dteta); /* dteta = teta_prev - teta_curr */
if (v_norm2(teta) == 0.0)
rel_step = 0.0;
else
rel_step = v_norm2(dteta) / v_norm2(teta);
} /* while (n_iter < gl_iter && rel_step > fit_limit) */
print_progress(max_iter, max_iter);
if (n_iter == gl_iter)
pr_warning("No convergence after %d iterations", n_iter);
if (DEBUG_VGMFIT) { /* calculate and report covariance matrix */
/* first, update to current est */
for (i = 0, m_zero(Vw); i < n_k; i++)
ms_mltadd(Vw, Vk[i], teta->ve[i], Vw); /* Vw = Sum_i teta[i]*V[i] */
VinvIminAw = calc_VinvIminAw(Vw, X, VinvIminAw, 0);
calc_rhs_Tr_m(n_k, Vk, VinvIminAw, Y, rhs, Tr_m);
m_inverse(Tr_m, Tr_m);
sm_mlt(2.0, Tr_m, Tr_m); /* Var(YAY)=2tr(AVAV) */
printlog("Lower bound of parameter covariance matrix:\n");
m_logoutput(Tr_m);
printlog("# Negative log-likelyhood: %g\n", calc_ll(Vw, X, Y, n_k));
}
m_free(Vw);
m_free(VinvIminAw);
m_free(Tr_m);
v_free(rhs);
v_free(dteta);
return (n_iter < max_iter && rel_step < fit_limit); /* converged? */
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
VEC *bisvd(VEC *d, VEC *f, MAT *U, MAT *V)
#endif
{
int i, j, n;
int i_min, i_max, split;
Real c, s, shift, size, z;
Real d_tmp, diff, t11, t12, t22, *d_ve, *f_ve;
if ( ! d || ! f )
error(E_NULL,"bisvd");
if ( d->dim != f->dim + 1 )
error(E_SIZES,"bisvd");
n = d->dim;
if ( ( U && U->n < n ) || ( V && V->m < n ) )
error(E_SIZES,"bisvd");
if ( ( U && U->m != U->n ) || ( V && V->m != V->n ) )
error(E_SQUARE,"bisvd");
if ( n == 1 )
{
if ( d->ve[0] < 0.0 )
{
d->ve[0] = - d->ve[0];
if ( U != MNULL )
sm_mlt(-1.0,U,U);
}
return d;
}
d_ve = d->ve; f_ve = f->ve;
size = v_norm_inf(d) + v_norm_inf(f);
i_min = 0;
while ( i_min < n ) /* outer while loop */
{
/* find i_max to suit;
submatrix i_min..i_max should be irreducible */
i_max = n - 1;
for ( i = i_min; i < n - 1; i++ )
if ( d_ve[i] == 0.0 || f_ve[i] == 0.0 )
{ i_max = i;
if ( f_ve[i] != 0.0 )
{
/* have to ``chase'' f[i] element out of matrix */
z = f_ve[i]; f_ve[i] = 0.0;
for ( j = i; j < n-1 && z != 0.0; j++ )
{
givens(d_ve[j+1],z, &c, &s);
s = -s;
d_ve[j+1] = c*d_ve[j+1] - s*z;
if ( j+1 < n-1 )
{
z = s*f_ve[j+1];
f_ve[j+1] = c*f_ve[j+1];
}
if ( U )
rot_rows(U,i,j+1,c,s,U);
}
}
break;
}
if ( i_max <= i_min )
{
i_min = i_max + 1;
continue;
}
/* printf("bisvd: i_min = %d, i_max = %d\n",i_min,i_max); */
split = FALSE;
while ( ! split )
{
/* compute shift */
t11 = d_ve[i_max-1]*d_ve[i_max-1] +
(i_max > i_min+1 ? f_ve[i_max-2]*f_ve[i_max-2] : 0.0);
t12 = d_ve[i_max-1]*f_ve[i_max-1];
t22 = d_ve[i_max]*d_ve[i_max] + f_ve[i_max-1]*f_ve[i_max-1];
/* use e-val of [[t11,t12],[t12,t22]] matrix
closest to t22 */
diff = (t11-t22)/2;
shift = t22 - t12*t12/(diff +
sgn(diff)*sqrt(diff*diff+t12*t12));
/* initial Givens' rotation */
givens(d_ve[i_min]*d_ve[i_min]-shift,
d_ve[i_min]*f_ve[i_min], &c, &s);
/* do initial Givens' rotations */
d_tmp = c*d_ve[i_min] + s*f_ve[i_min];
f_ve[i_min] = c*f_ve[i_min] - s*d_ve[i_min];
d_ve[i_min] = d_tmp;
z = s*d_ve[i_min+1];
d_ve[i_min+1] = c*d_ve[i_min+1];
if ( V )
rot_rows(V,i_min,i_min+1,c,s,V);
/* 2nd Givens' rotation */
givens(d_ve[i_min],z, &c, &s);
d_ve[i_min] = c*d_ve[i_min] + s*z;
d_tmp = c*d_ve[i_min+1] - s*f_ve[i_min];
f_ve[i_min] = s*d_ve[i_min+1] + c*f_ve[i_min];
d_ve[i_min+1] = d_tmp;
if ( i_min+1 < i_max )
{
z = s*f_ve[i_min+1];
f_ve[i_min+1] = c*f_ve[i_min+1];
}
if ( U )
rot_rows(U,i_min,i_min+1,c,s,U);
for ( i = i_min+1; i < i_max; i++ )
{
/* get Givens' rotation for zeroing z */
givens(f_ve[i-1],z, &c, &s);
f_ve[i-1] = c*f_ve[i-1] + s*z;
d_tmp = c*d_ve[i] + s*f_ve[i];
f_ve[i] = c*f_ve[i] - s*d_ve[i];
d_ve[i] = d_tmp;
z = s*d_ve[i+1];
d_ve[i+1] = c*d_ve[i+1];
if ( V )
rot_rows(V,i,i+1,c,s,V);
/* get 2nd Givens' rotation */
givens(d_ve[i],z, &c, &s);
d_ve[i] = c*d_ve[i] + s*z;
d_tmp = c*d_ve[i+1] - s*f_ve[i];
f_ve[i] = c*f_ve[i] + s*d_ve[i+1];
d_ve[i+1] = d_tmp;
if ( i+1 < i_max )
{
z = s*f_ve[i+1];
f_ve[i+1] = c*f_ve[i+1];
}
if ( U )
rot_rows(U,i,i+1,c,s,U);
}
/* should matrix be split? */
for ( i = i_min; i < i_max; i++ )
if ( fabs(f_ve[i]) <
MACHEPS*(fabs(d_ve[i])+fabs(d_ve[i+1])) )
{
split = TRUE;
f_ve[i] = 0.0;
}
else if ( fabs(d_ve[i]) < MACHEPS*size )
{
split = TRUE;
d_ve[i] = 0.0;
}
/* printf("bisvd: d =\n"); v_output(d); */
/* printf("bisvd: f = \n"); v_output(f); */
}
}
fixsvd(d,U,V);
return d;
}
