64 lines
2.4 KiB
C
64 lines
2.4 KiB
C
|
/******************** (C) COPYRIGHT 2020 Geek************************************
|
|||
|
|
* File Name : pdr_kalman.h
|
|||
|
|
* Department : Sensor Algorithm Team
|
|||
|
|
* Current Version : V2.0(compare QCOM SAP 5.0)
|
|||
|
|
* Author : logzhan
|
|||
|
|
* Date of Issued : 2020.7.3
|
|||
|
|
* Comments : PDR <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD>ͷ<EFBFBD>ļ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Լ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ؽṹ<EFBFBD>嶨<EFBFBD><EFBFBD>
|
|||
|
|
********************************************************************************/
|
|||
|
|
#ifndef _PDR_KALMAN_H_
|
|||
|
|
#define _PDR_KALMAN_H_
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
extern "C" {
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_initKalman
|
|||
|
|
* Description : <EFBFBD><EFBFBD>ʼ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* Date : 2020/8/27
|
|||
|
|
*
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void KalmanFilter_Init(void);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_ekfStatePredict
|
|||
|
|
* Description : pdr<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬Ԥ<EFBFBD>ⷽ<EFBFBD><EFBFBD>
|
|||
|
|
* Date : 2020/7/22 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void pdr_ekfStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : getQRValue
|
|||
|
|
* Description : <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* scane_type, <EFBFBD>û<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͷǿ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* nmea_data<EFBFBD><EFBFBD>NMEA<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|||
|
|
* g_pdr<EFBFBD><EFBFBD>PDR<EFBFBD>ṹ<EFBFBD><EFBFBD>
|
|||
|
|
* sys<EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|||
|
|
* kf<EFBFBD><EFBFBD>EKF<EFBFBD><EFBFBD><EFBFBD>ݽṹ<EFBFBD><EFBFBD>
|
|||
|
|
* Date : 2020/07/09 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void CalEKFQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_ekfStateUpdate
|
|||
|
|
* Description : pdr<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>״̬<EFBFBD><EFBFBD><EFBFBD>·<EFBFBD><EFBFBD><EFBFBD>
|
|||
|
|
* Date : 2020/7/22 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void EKFStateUpdate(KfPara_t* kf, GNSS_t* pgnss, classifer* sys, PDR* g_pdr,
|
|||
|
|
int scene_type);
|
|||
|
|
|
|||
|
|
/**----------------------------------------------------------------------
|
|||
|
|
* Function : pdr_gnssInsLocFusion
|
|||
|
|
* Description : PDR<EFBFBD><EFBFBD>GNSS<EFBFBD><EFBFBD>INS<EFBFBD>ں϶<EFBFBD>λ
|
|||
|
|
* Date : 2020/07/09 logzhan
|
|||
|
|
*---------------------------------------------------------------------**/
|
|||
|
|
void pdr_gnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias,
|
|||
|
|
KfPara_t* kf, lct_fs* res);
|
|||
|
|
|
|||
|
|
#ifdef __cplusplus
|
|||
|
|
}
|
|||
|
|
#endif
|
|||
|
|
|
|||
|
|
#endif
|