112 lines
3.5 KiB
C
112 lines
3.5 KiB
C
//
|
|
// Created by Wakanda_shaw on 2022/11/2.
|
|
//
|
|
|
|
#include "rtklib.h"
|
|
|
|
#define MINVEL 5.0 /* min velocity for initial ins states */
|
|
#define MAXGYRO (30.0*D2R) /* max rotation speed value for initial */
|
|
#define MICRO_G_2_MS 9.80665E-6 /* micro_g_to_meters_per_second_squared */
|
|
|
|
/* coarse alignment for ins navigation initial with stationaly imu measurement data
|
|
* args : insstate_t *ins IO ins states
|
|
* imud_t *data I imu measurement data (static case) {m/s^2,rad/s}
|
|
* int n I number of data (0: no use of data)
|
|
* insopt_t *opt I ins options
|
|
* return : 1:ok, 0:fail
|
|
* note : this model is suitable for small attitude error's
|
|
* ---------------------------------------------------------------------------*/
|
|
static int coarse_align(ins_t *ins,const imud_t *data,int n){
|
|
int i,j,k;
|
|
double pos[3],fb[3]={0},omg[3]={0},Cne[9],vn[3];
|
|
double roll,pitch,yaw;
|
|
|
|
trace(3,"coarse_asign: n=%d\n",n);
|
|
|
|
if (n<=0) return 0;
|
|
ecef2pos(ins->position,pos);
|
|
ned2xyz(pos,Cne);
|
|
|
|
matmul("TN",3,1,3,1.0,Cne,ins->velocity,0.0,vn);
|
|
yaw = vel2head(vn); /* yaw */
|
|
|
|
/* average imu measurement data for remove noise */
|
|
for (k=0,i=0;i<n;i++) {
|
|
for (k++,j=0;j<3;j++) {
|
|
fb[j]+=data[i].accl[j];
|
|
omg[j]+=data[i].gyro[j];
|
|
}
|
|
}
|
|
for (i=0;i<3;i++) fb[i]/=k,omg[i]/=k;
|
|
|
|
roll = atan2(-fb[1],-fb[2]);
|
|
pitch = atan(fb[0]/ sqrt(fb[1]*fb[1] + fb[2]*fb[2]));
|
|
|
|
|
|
ins->Cbn[0] = cos(pitch)* cos(yaw);
|
|
ins->Cbn[1] = cos(pitch) * sin(yaw);
|
|
ins->Cbn[2] = -sin(pitch);
|
|
ins->Cbn[3] = -cos(roll)* sin(yaw) + sin(roll)* sin(pitch) * cos(yaw);
|
|
ins->Cbn[4] = cos(roll) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
|
|
ins->Cbn[5] = sin(roll) * cos(pitch);
|
|
ins->Cbn[6] = sin(roll) * sin(yaw) + cos(roll) * sin(pitch) * cos(yaw);
|
|
ins->Cbn[7] = -sin(roll) * cos(yaw) + cos(roll) * sin(pitch) * sin(yaw);
|
|
ins->Cbn[8] = cos(roll) * cos(pitch);
|
|
|
|
/* normalization */
|
|
matmul("NN",3,3,3,1.0,Cne,ins->Cbn,1.0,ins->Cbe);
|
|
normdcm(ins->Cbe);
|
|
|
|
return 1;
|
|
}
|
|
|
|
/* initialize ins states use PVT---------------------------
|
|
* args:
|
|
* ins_t *ins IO ins_states
|
|
* pvt_t *pvt I pvt states
|
|
* imu_t *imu I imu data
|
|
* return: imu_index
|
|
* ------------------------------------------------*/
|
|
extern int ins_init_pvt(ins_t *ins, pvt_t *pvt, imu_t *imu){
|
|
int i;
|
|
int imu_index=0;
|
|
double vr[3]={0};
|
|
|
|
for(i=0;i<pvt->n-1;i++){
|
|
/* init ins time */
|
|
ins->time = pvt->data[i].tow;
|
|
/* align imu data */
|
|
while(ins->time!=imu->data[imu_index].tow){
|
|
imu_index++;
|
|
}
|
|
ins->imu_time = imu->data[imu_index].time;
|
|
for(int k=0;k<3;k++){
|
|
ins->fb[k] = imu->data[imu_index].accl[k];
|
|
ins->omgb[k] = imu->data[imu_index].gyro[k];
|
|
}
|
|
matcpy(vr,pvt->data[i].ecef_v,1,3);
|
|
if (norm(vr,3)<MINVEL) {
|
|
printf("velocity = %lf\n", norm(vr,3));
|
|
continue;
|
|
}
|
|
// /* check velocity is ok */
|
|
break;
|
|
}
|
|
if(i==pvt->n){
|
|
return -1;
|
|
}
|
|
|
|
for(int k=0;k<3;k++){
|
|
ins->velocity[k]=pvt->data[i].ecef_v[k];
|
|
ins->position[k]=pvt->data[i].ecef_pos[k];
|
|
ins->acc_bias[k]=0;
|
|
ins->gyro_bias[k]=0;
|
|
}
|
|
|
|
/* initial imu attitude */
|
|
if(!coarse_align(ins,imu->data,1000)){
|
|
trace(3, "coarse align fail\n");
|
|
}
|
|
|
|
return imu_index;
|
|
} |