DSP可以使用的GPS+卡尔曼滤波算法程序

//////////////////////////////////////////////////////////////////////////////////////////////////////// // // // 本程序实现GPS数据的滤波功能，GPS数据首先从SCI-B口进入，进行拆解滤波后 // // 再经由SCI-B送出。 // // // //////////////////////////////////////////////////////////////////////////////////////////////////////// #include "DSP2833x_Device.h" // DSP2833x Headerfile Include File #include "DSP2833x_Examples.h" // DSP2833x Examples Include File #include "MySCIConfig.h" //包含串口配置文件 #include "MyWDConfig.h" //包含看门狗配置文件 #include <string.h> #include <stdio.h> #include <stdlib.h> #include "KalmanFilter.h" //============================= 全局变量声明 ==============================// extern char SCIBRxBuffer []; //SCI-B接收缓存,外部变量 extern volatile Uint32 SCIB_RxPtrIn; //SCI-B当前接收地址,外部变量 extern volatile Uint32 SCIB_RxDone; //SCI-B是否接收完成,外部变量 extern char SCICRxBuffer []; //SCI-C接收缓存,外部变量 extern volatile Uint32 SCIC_RxPtrIn; //SCI-C当前接收地址,外部变量 extern volatile Uint32 SCIC_RxDone; //SCI-C是否接收完成,外部变量 char OutPutBuffer[120]; //输出缓存 int RetVal; //函数返回值 //GPS信息数据结构体 (GPRMC格式) /* typedef struct _GPRMCInfoStruct */ /* { */ /* char Header[8]; */ /* char UTCTime[15]; */ /* char PosStatus[2]; */ /* char Latitude[12]; */ /* char LaNS[2]; */ /* char Longtitude[12]; */ /* char LongWE[2]; */ /* char EarthRate[6]; */ /* char EarthDirect[8]; */ /* char UTCDate[10]; */ /* char RestInfo[60]; */ /* } GPRMCInfoStruct; */ //GPS信息数据结构体 (GPRMC格式) typedef struct _GPGGAInfoStruct { char Header[8]; //GPGGA头 char UTCTime[15]; //UTC时间 char Latitude[12]; //维度 char LaNS[2]; //纬度半球，N或S char Longtitude[12]; //经度 char LongWE[2]; //经度半球，W或E char PosStatus[2]; //GPS定位状态,0未定位， 1非差分定位， 2差分定位， 3无效PPS， 6正在估算 char SatNo[4]; //卫星数量 char HorizontalAccuracy [6]; //水平精准度 char RestInfo[60]; //其它数据 } GPGGAInfoStruct; /* GPRMCInfoStruct GPRMCInfo; */ GPGGAInfoStruct GPGGAInfo; long double ldUTCTime,ldLatitude,ldLongtitude; //UTC时间，纬度，经度，数值 //long double* OutLatitude; //输出纬度 //long double* OutLongtitude; //输出经度 long double OutLatitude; //输出纬度 long double OutLongtitude; //输出经度 //#define __GPRMC_GPS //__GPRMC_GPS与__GPGGA_GPS里选择一个 #define __GPGGA_GPS //__GPRMC_GPS与__GPGGA_GPS里选择一个 //============================= 函数原型声明 ==============================// interrupt void SCIBRxIntFunc(void); //SCI-B接收 中断服务函数. interrupt void SCICRxIntFunc(void); //SCI-C接收 中断服务函数. /* int ExtractGPRMCInfo(char* GPSString); //抽取GPS数据，将拆解后的数据放到GPRMCInfo里，成功返回0，否则返回-1 */ int ExtractGPGGAInfo(char* GPSString); //抽取GPS数据，将拆解后的数据放到GPGGAInfo里，成功返回0，否则返回-1 Matrix Kalman_A,Kalman_B,Kalman_Q,Kalman_H,Kalman_C,Kalman_S_k,Kalman_M_k,Kalman_x_k; Matrix Kalman_S_k1_Predict,Kalman_M_k1_Predict,Kalman_K,Kalman_S_k1,Kalman_M_k1,Kalman_NewMsg,Kalman_NewMsg_Cov; long double Kalman_q; //Kalman_q系数对Kalman_Q进行调整，一般较小为好 long double Kalman_c; //Kalman_c系数对Kalman_c进行调整,一般取较大为好 long double OutlierThreshold; //野值门限，自由度为2的卡方分布，右尾概率为0.001的中心化卡方值 long double Longitude,Latitude;//Longitude，上一次处理后的经度；Latitude，上一次处理后的纬度 //DataNull数据是否为空的标志，数据空为TRUE，否则是FALSE;KalmanStart卡尔曼滤波开始标志，当有第一个数据以后，卡尔曼滤波才能启动;OutlierOutput，表明该值是否由滤波Outlier得到 int DataNull, KalmanStart, OutlierOutput; long double PreLongitude,PreLatitude;//存储上一次的经纬度信息 long double PreTime; //存储上一次的时间 Matrix Kalman_Inv_NewMsg_Cov,TmpMat66_1,TmpMat66_2,TmpMat66_3,TmpMat66_4,TmpMat21_1,TmpMat26_1,TmpMat22_1,TmpMat62_1,TmpMat62_2,TmpMat61_1; void main(void) { // Step 1. Initialize System Control: // PLL, WatchDog, enable Peripheral Clocks // This example function is found in the DSP2833x_SysCtrl.c file. InitSysCtrl(); // Step 2. Initalize GPIO: // This example function is found in the DSP2833x_Gpio.c file and // illustrates how to set the GPIO to it's default state. // InitGpio(); Skipped for this example // For this example, only init the pins for the SCI-A port. // This function is found in the DSP2833x_Sci.c file. InitScibGpio(); InitScicGpio(); InitXintf16Gpio(); // Step 3. Clear all interrupts and initialize PIE vector table: // Disable CPU interrupts DINT; // Initialize PIE control registers to their default state. // The default state is all PIE interrupts disabled and flags // are cleared. // This function is found in the DSP2833x_PieCtrl.c file. InitPieCtrl(); // Disable CPU interrupts and clear all CPU interrupt flags: IER = 0x0000; IFR = 0x0000; // Initialize the PIE vector table with pointers to the shell Interrupt // Service Routines (ISR). // This will populate the entire table, even if the interrupt // is not used in this example. This is useful for debug purposes. // The shell ISR routines are found in DSP2833x_DefaultIsr.c. // This function is found in DSP2833x_PieVect.c. InitPieVectTable(); EALLOW; // This is needed to write to EALLOW protected registers PieVectTable.SCIRXINTB = &SCIBRxIntFunc;//SCI-B接收中断服务函数 PieVectTable.SCIRXINTC = &SCICRxIntFunc;//SCI-C接收中断服务函数 EDIS; // This is needed to disable write to EALLOW protected registers // Step 4. Initialize all the Device Peripherals: // This function is found in DSP2833x_InitPeripherals.c // InitPeripherals(); // Not required for this example // Step 5. User specific code: InitSCIBRecvInt(); //使能SCI-B的接收中断 InitSCICRecvInt(); //使能SCI-B的接收中断 EINT; SCIBFifoInit(); //SCI-B发送FIFO的初始化 SCIBParamInit(BAUDRATE_38400); //SCI-B参数配置初始化 SCIBSendMsg("SCI-B has been Initialized..\n"); SCIBSendMsg("SCI-B has been Initialized..\n"); SCICFifoInit(); //SCI-C发送FIFO的初始化 SCICParamInit(BAUDRATE_9600); //SCI-C参数配置初始化 SCICSendMsg("SCI-C has been Initialized..\n"); memset(OutPutBuffer,0,sizeof(OutPutBuffer)); ldUTCTime = 0.0L; ldLatitude = 0.0L; ldLongtitude = 0.0L; RetVal = 0; OutLatitude = 0.0L; OutLongtitude = 0.0L; // OutLatitude = NULL; // OutLongtitude = NULL; KalmanInit(); SCIBSendMsg("SCI-B has been Initialized After Kalman..\n"); SCIBSendMsg("SCI-B has been Initialized After Kalman..\n"); //初始化GPRMCInfo或者GPGGAInfo /* #ifdef __GPRMC_GPS */ /* memset(&GPRMCInfo,0,sizeof(GPRMCInfo)); */ /* #endif */ #ifdef __GPGGA_GPS memset(&GPGGAInfo,0,sizeof(GPGGAInfo)); #endif for(;;) { if (SCIB_RxDone == 1) //如果SCI-B字符串接收完毕 { if(strlen(SCIBRxBuffer)!= 0) { /* #ifdef __GPRMC_GPS */ /* */ /* RetVal = ExtractGPRMCInfo(SCIBRxBuffer); //抽取GPRMC信息 */ /* */ /* #endif */ /* #ifdef __GPRMC_GPS */ /* if (RetVal == 0) //如果成功拆解 */ /* {