63 lines
2.4 KiB
C
63 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 卡尔曼滤波器头文件函数声明以及相关结构体定义
|
||
********************************************************************************/
|
||
#ifndef _PDR_KALMAN_H_
|
||
#define _PDR_KALMAN_H_
|
||
|
||
#ifdef __cplusplus
|
||
extern "C" {
|
||
#endif
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EKF_Init
|
||
* Description : 初始化卡尔曼滤波器相关
|
||
* Date : 2022/09/19
|
||
*---------------------------------------------------------------------**/
|
||
void EKF_Init(void);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EKFStatePredict
|
||
* Description : pdr卡尔曼滤波器的状态预测方程
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKFStatePredict(KfPara_t* kf, double step_length, PDR* g_pdr, int step);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EKFCalQRMatrix
|
||
* Description : 计算卡尔曼滤波噪声矩阵
|
||
* scane_type, 用户所处场景（开阔和非开阔）
|
||
* nmea_data，NMEA数据结构体
|
||
* g_pdr，PDR结构体
|
||
* sys，运动幅度数据结构体
|
||
* kf，EKF数据结构体
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKFCalQRMatrix(lct_nmea* nmea_data, PDR* g_pdr, classifer* sys, KfPara_t* kf);
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_ekfStateUpdate
|
||
* Description : pdr卡尔曼滤波器的状态更新方程
|
||
* 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的GNSS和INS融合定位
|
||
* Date : 2020/07/09 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void GnssInsLocFusion(GNSS_t* pgnss, PDR* g_pdr, classifer* sys, double yaw_bias,
|
||
KfPara_t* kf, lct_fs* res);
|
||
|
||
#ifdef __cplusplus
|
||
}
|
||
#endif
|
||
|
||
#endif
|