int steepest_solver ( int n, double ** A, double * b, double * x ) { int row, col; double **AT, *ATb, **ATA; double *residual; AT = (double**) malloc (n*sizeof(double*)); ATb = (double*) malloc (n*sizeof(double)); ATA = (double**) malloc (n*sizeof(double*)); residual = (double*) malloc (n*sizeof(double)); // allocate for (row=0; row<n; row++) { AT[row] = (double*) malloc(n*sizeof(double)); ATA[row] = (double*) malloc(n*sizeof(double)); } // aux variables transpose (n, A, AT); mat_mat_mult (n, AT, A, ATA); mat_vec_mult (n, AT, b, ATb); // x is the vector that minimizes f(x) = 0.5(x^T)ATAx - ((ATb)^T)x // initialize int step = 0; for (row=0; row<n; row++) { residual[row] = ATb[row]; for (col=0; col<n; col++) residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant } // delta = rTr; double delta = vec_vec_mult (n,residual,residual); double delta_0 = delta; do { double alpha = delta / vec_mat_vec_mult (n, residual, ATA);// + DBL_EPSILON; for (row=0; row<n; row++) x[row] += alpha * residual[row]; for (row=0; row<n; row++) { residual[row] = ATb[row]; for (col=0; col<n; col++) residual[row] -= ATA[row][col]*x[col]; // in HCDC, x elements are non constant all but row number of terms are constant } delta = vec_vec_mult (n,residual,residual); step++; } while ( delta>DBL_EPSILON*delta_0 && step<(1<<20) ); for (row=0; row<n; row++) { free(AT[row]); free(ATA[row]); } free (AT); free (ATA); free (ATb); free (residual); return step; }
int lob_scaat(SCAATState *out, STORAGE_TYPE z, STORAGE_TYPE s_x, STORAGE_TYPE s_y, STORAGE_TYPE dt, SCAATState *in, STORAGE_TYPE q, STORAGE_TYPE R){ STORAGE_TYPE **A; STORAGE_TYPE **Q; STORAGE_TYPE **Q1; // temporary matrix STORAGE_TYPE opt_z=0.0; STORAGE_TYPE *H; STORAGE_TYPE *K; //Kalman gain STORAGE_TYPE **I; //unit matrix STORAGE_TYPE **v; // for the eigenvectors int *nrot; // for the Jacobi method of eigenvalue computation STORAGE_TYPE *d; SCAATState pred; // prediction int i, j; int add_noise = 0; // section for temporary variables STORAGE_TYPE *t_v1; // vector STORAGE_TYPE **t_m1; // matrix STORAGE_TYPE **t_m2; // matrix STORAGE_TYPE **t_m3; // matrix STORAGE_TYPE t_s; //temporary scalar value STORAGE_TYPE temp_sum = 0.0; STORAGE_TYPE min_d = 0.0; // program start //A=[1 0 dt 0 dt*dt/2 0 // 0 1 0 dt 0 dt*dt/2 // 0 0 1 0 dt 0 // 0 0 0 1 0 dt // 0 0 0 0 1 0 // 0 0 0 0 0 1]; pred.x = VECTOR(1, STATE_NUM); //pred.P = MATRIX(1, STATE_NUM, 1, STATE_NUM); pred.P = dmatrix_alloc(STATE_NUM,STATE_NUM); t_v1 = VECTOR(1, STATE_NUM); //t_m1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m2 = MATRIX(1, STATE_NUM, 1, STATE_NUM); //t_m3 = MATRIX(1, STATE_NUM, 1, STATE_NUM); t_m1 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m2 = dmatrix_alloc(STATE_NUM,STATE_NUM); t_m3 = dmatrix_alloc(STATE_NUM,STATE_NUM); //v = MATRIX(1, STATE_NUM, 1, STATE_NUM); v = dmatrix_alloc(STATE_NUM,STATE_NUM); nrot = ivector(1, STATE_NUM); d = VECTOR(1, STATE_NUM); //A = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q = MATRIX(1, STATE_NUM, 1, STATE_NUM); //Q1 = MATRIX(1, STATE_NUM, 1, STATE_NUM); A = dmatrix_alloc(STATE_NUM,STATE_NUM); Q = dmatrix_alloc(STATE_NUM,STATE_NUM); Q1 = dmatrix_alloc(STATE_NUM,STATE_NUM); H = VECTOR(1, STATE_NUM); K = VECTOR(1, STATE_NUM); //I = MATRIX(1, STATE_NUM, 1, STATE_NUM); I = dmatrix_alloc(STATE_NUM,STATE_NUM); // Initialize matrices A and Q for (i=1;i<=STATE_NUM;i++){ K[i] = 0.0; for (j=1;j<=STATE_NUM;j++){ A[i][i] = 0; Q[i][i] = 0; Q1[i][i] = 0; if (i==j) I[i][j]= 1.0; else I[i][j]= 0.0; } } for (i=1;i<=STATE_NUM;i++){ A[i][i] = 1; if (i+2<=STATE_NUM) A[i][i+2] = dt; if (i+4<=STATE_NUM) A[i][i+4] = dt*dt/2.0; } //% The state error covariance matrix is a function of the driving //% error which is assumed to only hit the accelaration directly. //% Indirectly this noise affects the velocity and position estimate //% through the dynamics model. //Q=q*[(dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 0 // 0 (dt^5)/20 0 (dt^4)/8 0 (dt^3)/6 // (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 0 // 0 (dt^4)/8 0 (dt^3)/3 0 (dt^2)/2 // (dt^3)/6 0 (dt^2)/2 0 dt 0 // 0 (dt^3)/6 0 (dt^2)/2 0 dt]; Q[1][1] = q*pow(dt, 5)/20.0; Q[1][3] = q*pow(dt, 4)/8.0; Q[1][5] = q*pow(dt, 3)/6.0; Q[2][2] = q*pow(dt, 5)/20.0; Q[2][4] = q*pow(dt, 4)/8.0; Q[2][6] = q*pow(dt, 3)/6.0; Q[3][1] = q*pow(dt, 4)/8.0; Q[3][3] = q*pow(dt, 3)/3.0; Q[3][5] = q*pow(dt, 2)/2.0; Q[4][2] = q*pow(dt, 4)/8.0; Q[4][4] = q*pow(dt, 3)/3.0; Q[4][6] = q*pow(dt, 2)/2.0; Q[5][1] = q*pow(dt, 3)/6.0; Q[5][3] = q*pow(dt, 2)/2.0; Q[5][5] = q*dt; Q[6][2] = q*pow(dt, 3)/6.0; Q[6][4] = q*pow(dt, 2)/2.0; Q[6][6] = q*dt; /*for (i=1;i<=STATE_NUM;i++) { for (j=1;j<=STATE_NUM;j++) printf("%f ",Q[i][j]); printf("\n"); }*/ //%step b //pred_x=A*pre_x; %6 x 1 mat_vec_mult(pred.x, A, in->x, STATE_NUM, 0); //pred_P=A*pre_P*(A')+Q; %6 x 6 // A is transpose mat_mat_mult(t_m1, in->P, A, STATE_NUM, 0, 1); mat_mat_mult(t_m2, A, t_m1, STATE_NUM, 0, 0); mat_add(pred.P, t_m2, Q, STATE_NUM, 0); // %step c // % assume that 0<=z<2*pi // opt_z = 2*pi + atan2((pred_x(2)-s_y), (pred_x(1)-s_x)); %1 x 1 // if (opt_z>2*pi) // opt_z = opt_z - 2*pi; // end // %H is 1 x 6 // % measurement function is nonlinear // % // % z = atan(x(2) - s_y, x(1) - s_x); // % In order to linearize the problem we find the jacobian // % noticing that // % // % d(atan(x))/dx = 1/(1+x^2) // % // H=[ -(pred_x(2)-s_y)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), (pred_x(1)-s_x)/(( (pred_x(1)-s_x)^2+(pred_x(2)-s_y)^2 )), 0, 0, 0, 0]; opt_z = 2*PI + atan2((pred.x[2]-s_y), (pred.x[1]-s_x)); H[1] = -(pred.x[2]-s_y)/ ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[2] = (pred.x[1]-s_x) / ( (pred.x[1]-s_x)*(pred.x[1]-s_x)+(pred.x[2]-s_y)*(pred.x[2]-s_y) ); H[3] = 0.0; H[4] = 0.0; H[5] = 0.0; H[6] = 0.0; //test=find(isnan(H)==1); //if (~isempty(test)) // disp('H goes to infinity due to node position is the same as the predicted value') // return; //end if (isnan(H[1]) || isnan(H[1])){ fprintf(stderr, "H goes to infinity due to node position is the same as the predicted value\n"); return(-1); } //%step d //%also test if P is still a positive definite matrix. If not, add some small noise to it to //%make it still positive. //n1=1; //Q1=n1*eye(6); //Q1(1,1)=0.1; //Q1(2,2)=0.1; //Q1(3,3)=5; //Q1(4,4)=5; Q1[1][1] = 0.1; Q1[2][2] = 0.1; Q1[3][3] = 5.0; Q1[4][4] = 5.0; Q1[5][5] = 1.0; Q1[6][6] = 1.0; //if isempty(find(eig(pred_P)<0.001)) // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //else // pred_P=pred_P+Q1; // K=pred_P*H'*inv(H*pred_P*H' + R); %6 x 1 //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m2, pred.P, STATE_NUM, 0); jacobi(t_m2, STATE_NUM, d, v, nrot); for(i=1;i<=STATE_NUM;i++){ if (d[i]< 0.001) add_noise=1; } if (add_noise){ mat_add(t_m1, pred.P, Q1, STATE_NUM, 0); mat_copy(pred.P, t_m1, STATE_NUM, 0); } mat_vec_mult(t_v1, pred.P, H, STATE_NUM, 1); inner_prod(&t_s, t_v1, H, STATE_NUM); t_s+= R; scal_vec_mult(K, 1.0/t_s, t_v1, STATE_NUM); //%step e and f //I=eye(6); //opt_x=pred_x+K*(z-opt_z); %6 x 1 scal_vec_mult(t_v1, z - opt_z, K, STATE_NUM); vec_add(out->x, pred.x, t_v1, STATE_NUM); // H: 1 x 6, K: 6 x 1 //%Joseph form to ward off round off problem //opt_P=(I-K*H)*pred_P*(I-K*H)'+K*R*K'; %6 x 6 scal_vec_mult(t_v1, R, K, STATE_NUM); // R*K' outer_prod(t_m1, K, t_v1, STATE_NUM); // K*(R*K') outer_prod(t_m2, K, H, STATE_NUM); // K*H mat_add(t_m3, I, t_m2, STATE_NUM, 1); // I-K*H mat_mat_mult(t_m2, pred.P, t_m3, STATE_NUM, 0, 1); mat_mat_mult(out->P, t_m3, t_m2, STATE_NUM, 0, 0); mat_add(t_m3, out->P, t_m1, STATE_NUM, 0); mat_copy(out->P, t_m3, STATE_NUM, 0); //% PRECAUTIONARY APPROACH. EVEN THOUGH IT HASN'T HAPPEN IN THE SIMULATION SO FAR //% also test if opt_P is still a positive definite matrix. If not, add some small noise to it to //% make it still positive. //% d: a diagonal matrix of eigenvalues //% v: matrix whose vectors are the eigenvectors //% X*V = V*D //if isempty(find(eig(opt_P)<0.001)) //else //[v d]=eig(opt_P); //c=find(d==min(diag(d))); //d(c)=0.001; //opt_P=v*d*v'; //end add_noise=0; // jacobi destroys the input vector mat_copy(t_m1, out->P, STATE_NUM, 0); jacobi(t_m1, STATE_NUM, d, v, nrot); min_d = d[1] ; for(i=1;i<=STATE_NUM;i++){ if ( min_d > d[i]) min_d = d[i]; if (d[i]< 0.001) add_noise=1; } // CAUTION - maybe the 2nd minimum is quite far from 0.001 if (add_noise){ // change diagonal matrix d for (i=1;i<STATE_NUM;i++){ if (d[i] == min_d) d[i] = 0.001; } // opt_P=v*d*v'; mat_copy(t_m1, v, STATE_NUM, 1); convert_vec_diag(t_m2, d, STATE_NUM); mat_mat_mult(t_m3, t_m2, t_m1, STATE_NUM, 0, 0); mat_mat_mult(out->P, v, t_m3, STATE_NUM, 0, 0); } //printf("The pred are x = %f y = %f\n",pred.x[1],pred.x[2]); // Free all MALLOCed matrices FREE_VECTOR_FUNCTION(pred.x, 1, STATE_NUM); FREE_MATRIX_FUNCTION(pred.P, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(t_v1, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m1, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m2, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(t_m3, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(v, 1, STATE_NUM, 1, STATE_NUM); free_ivector(nrot, 1, STATE_NUM); FREE_VECTOR_FUNCTION(d, 1, STATE_NUM); FREE_MATRIX_FUNCTION(A, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q, 1, STATE_NUM, 1, STATE_NUM); FREE_MATRIX_FUNCTION(Q1, 1, STATE_NUM, 1, STATE_NUM); FREE_VECTOR_FUNCTION(H, 1, STATE_NUM); FREE_VECTOR_FUNCTION(K, 1, STATE_NUM); FREE_MATRIX_FUNCTION(I, 1, STATE_NUM, 1, STATE_NUM); return(0); }