static void init_ins_state(void){
ins_logfile = fopen(INS_LOG_FILE, "w");
ins.avg_state.gyro_bias = bias_0;
ins.avg_state.orientation = orientation_0;
ins.avg_state.position = pos_0_ecef;
ins.avg_state.velocity = speed_0_ecef;
struct DoubleQuat ecef2body;
struct DoubleEulers eu_ecef2body;
QUATERNIOND_AS_DOUBLEQUAT(ecef2body, orientation_0);
DOUBLE_EULERS_OF_QUAT(eu_ecef2body, ecef2body);
printf("Initial state\n\n");
printf("Bias % 6.1f°/s +-%7.2f°/s\n", bias_0(0)*180*M_1_PI, bias_cov_0(0)*180*M_1_PI);
printf("Bias % 6.1f°/s +-%7.2f°/s\n", bias_0(1)*180*M_1_PI, bias_cov_0(1)*180*M_1_PI);
printf("Bias % 6.1f°/s +-%7.2f°/s\n", bias_0(2)*180*M_1_PI, bias_cov_0(2)*180*M_1_PI);
printf("\n");
printf("Orientation % 7.2f\n", orientation_0.w());
printf("Orientation % 7.2f %6.1f° +-%6.1f°\n", orientation_0.x(), eu_ecef2body.phi*180*M_1_PI, orientation_cov_0(0)*180*M_1_PI);
printf("Orientation % 7.2f %6.1f° +-%6.1f°\n", orientation_0.y(), eu_ecef2body.theta*180*M_1_PI, orientation_cov_0(1)*180*M_1_PI);
printf("Orientation % 7.2f %6.1f° +-%6.1f°\n", orientation_0.z(), eu_ecef2body.psi*180*M_1_PI, orientation_cov_0(2)*180*M_1_PI);
printf("\n");
printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(0), pos_cov_0(0));
printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(1), pos_cov_0(1));
printf("Position % 9.0f m +-%7.2f\n", pos_0_ecef(2), pos_cov_0(2));
printf("\n");
printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(0), speed_cov_0(0));
printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(1), speed_cov_0(1));
printf("Velocity % 7.2f m/s +-%7.2f\n", speed_0_ecef(2), speed_cov_0(2));
printf("\n");
Matrix<double, 12, 1> diag_cov;
/*diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 ,
//Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 ,
Vector3d::Ones() * angle_cov * angle_cov ,
Vector3d::Ones() * pos_cov_0 * pos_cov_0 ,
Vector3d::Ones() * speed_cov_0 * speed_cov_0;*/
diag_cov << gyro_stability_noise,
orientation_cov_0,
pos_cov_0,
speed_cov_0;
ins.cov = (diag_cov.cwise()*diag_cov).asDiagonal();
}
static void init_ins_state(void){
ins_logfile = fopen(INS_LOG_FILE, "w");
LLA_ASSIGN(pos_0_lla, TOULOUSE_LATTITUDE, TOULOUSE_LONGITUDE, TOULOUSE_HEIGHT)
PPRZ_LLA_TO_EIGEN_ECEF(pos_0_lla, pos_0_ecef);
printf("Starting position\t%f\t%f\t%f\n", pos_0_ecef(0), pos_0_ecef(1), pos_0_ecef(2));
ins.avg_state.position = pos_0_ecef;
ins.avg_state.gyro_bias = Vector3d::Zero();
ins.avg_state.orientation = Quaterniond::Identity();
ins.avg_state.velocity = speed_0_ecef;
Matrix<double, 12, 1> diag_cov;
diag_cov << Vector3d::Ones() * bias_cov_0 * bias_cov_0 ,
Vector3d::Ones() * M_PI*0.5 * M_PI*0.5 ,
Vector3d::Ones() * pos_cov_0 * pos_cov_0 ,
Vector3d::Ones() * speed_cov_0 * speed_cov_0;
ins.cov = diag_cov.asDiagonal();
}