Ejemplo n.º 1
0
// Main get_nav filter function
void get_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){
	double tnow, imu_dt;
	double dq[4], quat_new[4];

	// compute time-elapsed 'dt'
	// This compute the navigation state at the DAQ's Time Stamp
	tnow = sensorData_ptr->imuData_ptr->time;
	imu_dt = tnow - tprev;
	tprev = tnow;		
	
	// ==================  Time Update  ===================
	// Temporary storage in Matrix form
	quat[0] = navData_ptr->quat[0];
	quat[1] = navData_ptr->quat[1];
	quat[2] = navData_ptr->quat[2];
	quat[3] = navData_ptr->quat[3];
	
	a_temp31[0][0] = navData_ptr->vn; a_temp31[1][0] = navData_ptr->ve;
	a_temp31[2][0] = navData_ptr->vd;
	
	b_temp31[0][0] = navData_ptr->lat; b_temp31[1][0] = navData_ptr->lon;
	b_temp31[2][0] = navData_ptr->alt;
	
	// AHRS Transformations
	C_N2B = quat2dcm(quat, C_N2B);
	C_B2N = mat_tran(C_N2B, C_B2N);
	
	// Attitude Update
	// ... Calculate Navigation Rate
	nr = navrate(a_temp31,b_temp31,nr);
	
	dq[0] = 1;
	dq[1] = 0.5*om_ib[0][0]*imu_dt;
	dq[2] = 0.5*om_ib[1][0]*imu_dt;
	dq[3] = 0.5*om_ib[2][0]*imu_dt;
	
	qmult(quat,dq,quat_new);
	
	quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
	
    if(quat[0] < 0) {
        // Avoid quaternion flips sign
        quat[0] = -quat[0];
        quat[1] = -quat[1];
        quat[2] = -quat[2];
        quat[3] = -quat[3];
    }
    
	navData_ptr->quat[0] = quat[0];
	navData_ptr->quat[1] = quat[1];
	navData_ptr->quat[2] = quat[2];
	navData_ptr->quat[3] = quat[3];
	
	quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
	
	// Velocity Update
	dx = mat_mul(C_B2N,f_b,dx);
	dx = mat_add(dx,grav,dx);
	navData_ptr->vn += imu_dt*dx[0][0];
	navData_ptr->ve += imu_dt*dx[1][0];
	navData_ptr->vd += imu_dt*dx[2][0];
	
	// Position Update
	dx = llarate(a_temp31,b_temp31,dx);
	navData_ptr->lat += imu_dt*dx[0][0];
	navData_ptr->lon += imu_dt*dx[1][0];
	navData_ptr->alt += imu_dt*dx[2][0];
	
	// JACOBIAN
	F = mat_fill(F, ZERO_MATRIX);
	// ... pos2gs
	F[0][3] = 1.0; 	F[1][4] = 1.0; 	F[2][5] = 1.0;
	// ... gs2pos
	F[5][2] = -2*g/EARTH_RADIUS;
	
	// ... gs2att
	temp33 = sk(f_b,temp33);
	atemp33 = mat_mul(C_B2N,temp33,atemp33);
	
	F[3][6] = -2.0*atemp33[0][0]; F[3][7] = -2.0*atemp33[0][1]; F[3][8] = -2.0*atemp33[0][2];
	F[4][6] = -2.0*atemp33[1][0]; F[4][7] = -2.0*atemp33[1][1]; F[4][8] = -2.0*atemp33[1][2];
	F[5][6] = -2.0*atemp33[2][0]; F[5][7] = -2.0*atemp33[2][1]; F[5][8] = -2.0*atemp33[2][2];
	
	// ... gs2acc
	F[3][9] = -C_B2N[0][0]; F[3][10] = -C_B2N[0][1]; F[3][11] = -C_B2N[0][2];
	F[4][9] = -C_B2N[1][0]; F[4][10] = -C_B2N[1][1]; F[4][11] = -C_B2N[1][2];
	F[5][9] = -C_B2N[2][0]; F[5][10] = -C_B2N[2][1]; F[5][11] = -C_B2N[2][2];
	
	// ... att2att
	temp33 = sk(om_ib,temp33);
	F[6][6] = -temp33[0][0]; F[6][7] = -temp33[0][1]; F[6][8] = -temp33[0][2];
	F[7][6] = -temp33[1][0]; F[7][7] = -temp33[1][1]; F[7][8] = -temp33[1][2];
	F[8][6] = -temp33[2][0]; F[8][7] = -temp33[2][1]; F[8][8] = -temp33[2][2];
	
	// ... att2gyr
	F[6][12] = -0.5;
	F[7][13] = -0.5;
	F[8][14] = -0.5;
	
	// ... Accel Markov Bias
	F[9][9] = -1.0/TAU_A; 	F[10][10] = -1.0/TAU_A;	F[11][11] = -1.0/TAU_A;
	F[12][12] = -1.0/TAU_G; F[13][13] = -1.0/TAU_G;	F[14][14] = -1.0/TAU_G;
	
	//fprintf(stderr,"Jacobian Created\n");
	
	// State Transition Matrix: PHI = I15 + F*dt;
	temp1515 = mat_scalMul(F,imu_dt,temp1515);
	PHI = mat_add(I15,temp1515,PHI);
	
	// Process Noise
	G = mat_fill(G, ZERO_MATRIX);
	G[3][0] = -C_B2N[0][0];	G[3][1] = -C_B2N[0][1]; G[3][2] = -C_B2N[0][2];
	G[4][0] = -C_B2N[1][0];	G[4][1] = -C_B2N[1][1]; G[4][2] = -C_B2N[1][2];
	G[5][0] = -C_B2N[2][0];	G[5][1] = -C_B2N[2][1]; G[5][2] = -C_B2N[2][2];
	
	G[6][3] = -0.5;
	G[7][4] = -0.5;
	G[8][5] = -0.5;
	
	G[9][6] = 1.0; 			G[10][7] = 1.0; 		G[11][8] = 1.0;
	G[12][9] = 1.0; 		G[13][10] = 1.0; 		G[14][11] = 1.0;
	//fprintf(stderr,"Process Noise Matrix G is created\n");
	// Discrete Process Noise
	temp1512 = mat_mul(G,Rw,temp1512);
	temp1515 = mat_transmul(temp1512,G,temp1515);	// Qw = G*Rw*G'
	Qw = mat_scalMul(temp1515,imu_dt,Qw);			// Qw = dt*G*Rw*G'
	Q = mat_mul(PHI,Qw,Q);						// Q = (I+F*dt)*Qw
	
	temp1515 = mat_tran(Q,temp1515);
	Q = mat_add(Q,temp1515,Q);
	Q = mat_scalMul(Q,0.5,Q);				// Q = 0.5*(Q+Q')
	//fprintf(stderr,"Discrete Process Noise is created\n");
	
	// Covariance Time Update
	temp1515 = mat_mul(PHI,P,temp1515);
	P = mat_transmul(temp1515,PHI,P); 		// P = PHI*P*PHI'
	P = mat_add(P,Q,P);						// P = PHI*P*PHI' + Q
	temp1515 = mat_tran(P, temp1515);
	P = mat_add(P,temp1515,P);
	P = mat_scalMul(P,0.5,P);				// P = 0.5*(P+P')
	//fprintf(stderr,"Covariance Updated through Time Update\n");
	
	navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
	navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
	navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
	navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
	navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
	
	navData_ptr->err_type = TU_only;
	//fprintf(stderr,"Time Update Done\n");
	// ==================  DONE TU  ===================
	
	if(sensorData_ptr->gpsData_ptr->newData){
	
		// ==================  GPS Update  ===================
		sensorData_ptr->gpsData_ptr->newData = 0; // Reset the flag
		
		// Position, converted to NED
		a_temp31[0][0] = navData_ptr->lat;
		a_temp31[1][0] = navData_ptr->lon; a_temp31[2][0] = navData_ptr->alt;
		pos_ins_ecef = lla2ecef(a_temp31,pos_ins_ecef);
		
		a_temp31[2][0] = 0.0;
		//pos_ref = lla2ecef(a_temp31,pos_ref);
		pos_ref = mat_copy(a_temp31,pos_ref);
		pos_ins_ned = ecef2ned(pos_ins_ecef,pos_ins_ned,pos_ref);
		
		pos_gps[0][0] = sensorData_ptr->gpsData_ptr->lat*D2R;
		pos_gps[1][0] = sensorData_ptr->gpsData_ptr->lon*D2R;
		pos_gps[2][0] = sensorData_ptr->gpsData_ptr->alt;
		
		pos_gps_ecef = lla2ecef(pos_gps,pos_gps_ecef);
		
		pos_gps_ned = ecef2ned(pos_gps_ecef,pos_gps_ned,pos_ref);
		
		// Create Measurement: y
		y[0][0] = pos_gps_ned[0][0] - pos_ins_ned[0][0];
		y[1][0] = pos_gps_ned[1][0] - pos_ins_ned[1][0];
		y[2][0] = pos_gps_ned[2][0] - pos_ins_ned[2][0];
		
		y[3][0] = sensorData_ptr->gpsData_ptr->vn - navData_ptr->vn;
		y[4][0] = sensorData_ptr->gpsData_ptr->ve - navData_ptr->ve;
		y[5][0] = sensorData_ptr->gpsData_ptr->vd - navData_ptr->vd;
		
		//fprintf(stderr,"Measurement Matrix, y, created\n");
		
		// Kalman Gain
		temp615 = mat_mul(H,P,temp615);
		temp66 = mat_transmul(temp615,H,temp66);
		atemp66 = mat_add(temp66,R,atemp66);
		temp66 = mat_inv(atemp66,temp66); // temp66 = inv(H*P*H'+R)
		//fprintf(stderr,"inv(H*P*H'+R) Computed\n");
		
		temp156 = mat_transmul(P,H,temp156); // P*H'
		//fprintf(stderr,"P*H' Computed\n");
		K = mat_mul(temp156,temp66,K);	   // K = P*H'*inv(H*P*H'+R)
		//fprintf(stderr,"Kalman Gain Computed\n");
		
		// Covariance Update
		temp1515 = mat_mul(K,H,temp1515);
		ImKH = mat_sub(I15,temp1515,ImKH);	// ImKH = I - K*H
		
		temp615 = mat_transmul(R,K,temp615);
		KRKt = mat_mul(K,temp615,KRKt);		// KRKt = K*R*K'
		
		temp1515 = mat_transmul(P,ImKH,temp1515);
		P = mat_mul(ImKH,temp1515,P);		// ImKH*P*ImKH'
		temp1515 = mat_add(P,KRKt,temp1515);
		P = mat_copy(temp1515,P);			// P = ImKH*P*ImKH' + KRKt
		//fprintf(stderr,"Covariance Updated through GPS Update\n");
		
		navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2];
		navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5];
		navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8];
		navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11];
		navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14];
		
		// State Update
		x = mat_mul(K,y,x);
		denom = (1.0 - (ECC2 * sin(navData_ptr->lat) * sin(navData_ptr->lat)));
		denom = sqrt(denom*denom);

		Re = EARTH_RADIUS / sqrt(denom);
		Rn = EARTH_RADIUS*(1-ECC2) / denom*sqrt(denom);
		navData_ptr->alt = navData_ptr->alt - x[2][0];
		navData_ptr->lat = navData_ptr->lat + x[0][0]/(Re + navData_ptr->alt);
		navData_ptr->lon = navData_ptr->lon + x[1][0]/(Rn + navData_ptr->alt)/cos(navData_ptr->lat);
		
		navData_ptr->vn = navData_ptr->vn + x[3][0];
		navData_ptr->ve = navData_ptr->ve + x[4][0];
		navData_ptr->vd = navData_ptr->vd + x[5][0];
		
		quat[0] = navData_ptr->quat[0];
		quat[1] = navData_ptr->quat[1];
		quat[2] = navData_ptr->quat[2];
		quat[3] = navData_ptr->quat[3];
		
		// Attitude correction
		dq[0] = 1.0;
		dq[1] = x[6][0];
		dq[2] = x[7][0];
		dq[3] = x[8][0];
		
		qmult(quat,dq,quat_new);
		
		quat[0] = quat_new[0]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[1] = quat_new[1]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[2] = quat_new[2]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		quat[3] = quat_new[3]/sqrt(quat_new[0]*quat_new[0] + quat_new[1]*quat_new[1] + quat_new[2]*quat_new[2] + quat_new[3]*quat_new[3]);
		
		navData_ptr->quat[0] = quat[0];
		navData_ptr->quat[1] = quat[1];
		navData_ptr->quat[2] = quat[2];
		navData_ptr->quat[3] = quat[3];
		
		quat2eul(navData_ptr->quat,&(navData_ptr->phi),&(navData_ptr->the),&(navData_ptr->psi));
		
		navData_ptr->ab[0] = navData_ptr->ab[0] + x[9][0];
		navData_ptr->ab[1] = navData_ptr->ab[1] + x[10][0];
		navData_ptr->ab[2] = navData_ptr->ab[2] + x[11][0];
		
		navData_ptr->gb[0] = navData_ptr->gb[0] + x[12][0];
		navData_ptr->gb[1] = navData_ptr->gb[1] + x[13][0];
		navData_ptr->gb[2] = navData_ptr->gb[2] + x[14][0];
		
		navData_ptr->err_type = gps_aided;
		//fprintf(stderr,"Measurement Update Done\n");
	}
	
	// Remove current estimated biases from rate gyro and accels
	sensorData_ptr->imuData_ptr->p -= navData_ptr->gb[0];
	sensorData_ptr->imuData_ptr->q -= navData_ptr->gb[1];
	sensorData_ptr->imuData_ptr->r -= navData_ptr->gb[2];
	sensorData_ptr->imuData_ptr->ax -= navData_ptr->ab[0];
	sensorData_ptr->imuData_ptr->ay -= navData_ptr->ab[1];
	sensorData_ptr->imuData_ptr->az -= navData_ptr->ab[2];

	// Get the new Specific forces and Rotation Rate,
	// use in the next time update
	f_b[0][0] = sensorData_ptr->imuData_ptr->ax;
	f_b[1][0] = sensorData_ptr->imuData_ptr->ay;
	f_b[2][0] = sensorData_ptr->imuData_ptr->az;

	om_ib[0][0] = sensorData_ptr->imuData_ptr->p;
	om_ib[1][0] = sensorData_ptr->imuData_ptr->q;
	om_ib[2][0] = sensorData_ptr->imuData_ptr->r;


}
Ejemplo n.º 2
0
void varinit(void)
{
	int i;
	/*
	 * Resetting all flags
	 */
	Intr1_Cnt=0;
	Intr2_Cnt=0;
	IRQ1Flag = 1;
	IRQ2Flag = 1;

	WSZ = 34;
	TA_cnt =0;


	count = 0;
	qcnt = 0;

	velcnt = 0;
	rtime = 0.0;
	rcnt = 0;
	cnt_10ms = 0;




	latm = MasterLat;
	longm = MasterLon;

	epsilon = 0.0;
	four_delt = 4.0 * del_t;
	eight_delt = 8.0 * del_t;
	cdr_delt = cdr * del_t;
	cdr_delt_ms = cdr_delt / 3600;

	for(i=0;i<32;i++){
		Array_SA[i] = 0;
	}

	for(i=0;i<3;i++)
	{
		velo_ref_y[i] = 0.0;
		velo_ref_yold[i] = 0.0;;
		velo_ref_x[i] = 0.0;
		velo_ref_xold[i] = 0.0;

		pure_vel[i] = 0.0;

		p_velo_20ms[i] = 0.0;
		p_velo[i] = 0.0;

		pure_v_old[i] = 0.0;
		p_Ang[i] = 0.0;

		pure_gyro_drift[i] = 0.0;
		pure_acc_residu[i] = 0.0;

	}

#if 0

	/* these are known misalignment angles between M and S -
	 * Measured w.r.t Master to give DCM from slave to Master.
	 * Beware they are not between slave to NED */
	known_si    =  0.0 * cdr;
	known_theta =  0.0 * cdr;
	known_phi   =  0.0 * cdr;

	euler2dcm_stp(0, 0, 0, (double*)CSkew_est);
	transpose(3, 3, (double*)CSkew_est, (double*)CSkew_est_T);

	euler2dcm_stp(known_si, known_theta, known_phi, (double*)CS2M_K);
	transpose(3, 3, (double*)CS2M_K, (double*)CM2S_K);

	euler2dcm_stp(THDG, PITCH, ROLL, (double*)Cb2ned_M);
	matmul(3, 3, (double*)Cb2ned_M, 3, 3, (double*)CS2M_K, (double*)Cb2ned_S);

	if(ta_flag==1 && nav_flag==1)

	{
		dcm2quat((double*)Cb2ned_S, (double *)p_q_body2ned);

	}

	else if(ta_flag ==0 && level_flag==1)
#endif
	{

		euler2quat_spt(mdl_si,mdl_phi,mdl_theta,(double *)p_q_body2ned);


		p_si = mdl_si;
		p_phi = mdl_phi;
		p_theta = mdl_theta;



	}

	ned2ecef_q(latm, longm,(double*) q_ned2ecef);
	quat_mult((double*)q_ned2ecef,(double*)p_q_body2ned, (double*)p_q_body2ecef);


	/*
	 * Modification after Manjit discussion
	 */
	quat2dcm((double *)p_q_body2ecef,(double*)p_dcm);


	quat2dcm((double *)q_ned2ecef,(double*)p_dcm_n);
	matmul(3,3, (double*)p_dcm_n,3,1,(double*)MasterVel,(double*)pure_vel);




	pure_v_old[0] = pure_vel[0];
	pure_v_old[1] = pure_vel[1];
	pure_v_old[2] = pure_vel[2];

	init(0.0, 0.0, 0.0, p_velo_20ms);

	init(0.0, 0.0, 0.0, p_velo);


	init(0.0,0.0,0.0,pure_gyro_drift);
	init(0.0,0.0,0.0,pure_acc_residu);



	for (i = 0; i < 3; i++)
	{
		p_alp1[i] = 0.0;    p_alp2[i] = 0.0;    p_alp3[i] = 0.0;    p_alp4[i] = 0.0;

	}

	for (i = 0; i < 3; i++)
		Delta_Angle[i] = 0.0;

	for (i = 0; i < 6; i++)
		accum1[i] = 0.0;

	init(0.0, 0.0, earth_rate, omega);	 //earth rate vector ECEF

	//used in levelling
	Ned_omega[0] = earth_rate * cos(latm);
	Ned_omega[1] = 0.0;
	Ned_omega[2] = -earth_rate *sin(latm);

	for (i = 0; i < 3; i++)
		omg_dub[i] = 2.0 * omega[i];

	r_init = r0 * (1.0 - eccen * (sin(latm) * sin(latm)));


	pure_R = r_init + MasterAlt; // altitude;


	lla2ecef(latm,longm,MasterAlt,(double *)pure_ecef_pos); //input is geodetic



	pure_g_ecef();

	/****  for epsilon estimation   ****/

	init(0.0, 0.0, -pure_g_ecef_mag, Ned_gravity_detic);

}								 //end of varinit()