检查了宏定义和函数分号和括号成对等问题,但是语法没问题,求助各位大佬不知道问题出在哪里。以下是KF.h代码:
#ifndef __KF_H
#define __KF_H
#include <stdint.h>
#include <math.h>
#include "sys.h"
#define PI 3.14159265358979323846
#define WIE 7.292115e-5
#define R0 6378137.0
#define E 0.0818191908426
#define D2R (PI / 180.0)
#define R2D (180.0 / PI)
// GPS数据结构
typedef struct {
uint16_t year;
uint8_t month;
uint8_t date;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint32_t latitude;
uint8_t nshemi;
uint32_t longitude;
uint8_t ewhemi;
int altitude;
uint16_t speed;
} GPSData;
// IMU数据结构
typedef struct {
float fAcc[3];
float fGyro[3];
float fAngle[3];
} IMUData;
// 惯导状态结构体
typedef struct {
double cnb[3][3];
double q[4];
double att[3];
double vn[3];
double pos[3];
double ts;
double wie;
double r0;
double e;
double Rm;
double Rn;
double g[3];
} INSState;
// 函数声明
void a2mat(const double att[3], double Cnb[3][3]);
void askew(const double v[3], double S[3][3]);
void invbc(const double A[3][3], double invA[3][3]);
void m2att(double Cnb[3][3], double att[3]);
void m2qua(double Cnb[3][3], double qnb[4]);
void q2mat(const double qnb[4], double Cnb[3][3]);
void q2att(const double qnb[4], double att[3]);
void qmul(const double q1[4], const double q2[4], double q[4]);
void qupdt(const double q[4], const double phim[3], double q_new[4]);
void rv2q(const double rv[3], double q[4]);
void ins_init(INSState *ins, const GPSData *gps, const IMUData *imu);
void ins_update(INSState *ins, const IMUData *imu);
void kalman_filter(INSState *ins, const GPSData *gps);
#endif
#ifndef __KF_H
#define __KF_H
#include <stdint.h>
#include <math.h>
#include "sys.h"
#define PI 3.14159265358979323846
#define WIE 7.292115e-5
#define R0 6378137.0
#define E 0.0818191908426
#define D2R (PI / 180.0)
#define R2D (180.0 / PI)
// GPS数据结构
typedef struct {
uint16_t year;
uint8_t month;
uint8_t date;
uint8_t hour;
uint8_t min;
uint8_t sec;
uint32_t latitude;
uint8_t nshemi;
uint32_t longitude;
uint8_t ewhemi;
int altitude;
uint16_t speed;
} GPSData;
// IMU数据结构
typedef struct {
float fAcc[3];
float fGyro[3];
float fAngle[3];
} IMUData;
// 惯导状态结构体
typedef struct {
double cnb[3][3];
double q[4];
double att[3];
double vn[3];
double pos[3];
double ts;
double wie;
double r0;
double e;
double Rm;
double Rn;
double g[3];
} INSState;
// 函数声明
void a2mat(const double att[3], double Cnb[3][3]);
void askew(const double v[3], double S[3][3]);
void invbc(const double A[3][3], double invA[3][3]);
void m2att(double Cnb[3][3], double att[3]);
void m2qua(double Cnb[3][3], double qnb[4]);
void q2mat(const double qnb[4], double Cnb[3][3]);
void q2att(const double qnb[4], double att[3]);
void qmul(const double q1[4], const double q2[4], double q[4]);
void qupdt(const double q[4], const double phim[3], double q_new[4]);
void rv2q(const double rv[3], double q[4]);
void ins_init(INSState *ins, const GPSData *gps, const IMUData *imu);
void ins_update(INSState *ins, const IMUData *imu);
void kalman_filter(INSState *ins, const GPSData *gps);
#endif