694 lines
20 KiB
C
694 lines
20 KiB
C
/******************** (C) COPYRIGHT 2020 Geek************************************
|
||
* File Name : pdr_kalman.c
|
||
* Current Version : V2.0(compare QCOM SAP 5.0)
|
||
* Author : logzhan
|
||
* Date of Issued : 2020.7.3
|
||
* Comments : PDR 卡尔曼滤波器相关
|
||
********************************************************************************/
|
||
#include <stdio.h>
|
||
#include <string.h>
|
||
#include <math.h>
|
||
#if !defined(LOCATION_PLATFORM_QCOM_MODEM)
|
||
#include <malloc.h>
|
||
#endif
|
||
#include "pdr_base.h"
|
||
#include "Kalman.h"
|
||
#include "Location.h"
|
||
#include "Matrix.h"
|
||
#include "Utils.h"
|
||
#include "pdr_linearFit.h"
|
||
#include "CoordTrans.h"
|
||
|
||
// 扩展卡尔曼向量缓存
|
||
static double EkfVecBuf[5][N] = { {0.0} };
|
||
// 扩展卡尔曼矩阵缓存
|
||
static double EkfMatBuf[7][N][N] = { { {0.0} } };
|
||
|
||
extern double GpsPos[HIST_GPS_NUM][3];
|
||
|
||
extern int debugCount;
|
||
extern double angle_mean[3];
|
||
|
||
static int hold_type;
|
||
static double b_timestamp;
|
||
static double b_last_time;
|
||
static double attitude_yaw;
|
||
static DETECTOR_t *g_detector;
|
||
static int hold_type;
|
||
static double BUL_duration;
|
||
static double FUR_duration;
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : KalmanFilter_Init
|
||
* Description : 初始化卡尔曼滤波器相关
|
||
* Date : 2022/09/16 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKF_Init(void)
|
||
{
|
||
b_timestamp = -1;
|
||
b_last_time = -1;
|
||
BUL_duration = -1;
|
||
FUR_duration = -1;
|
||
attitude_yaw = -1;
|
||
hold_type = 0;
|
||
}
|
||
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : pdr_ekfStatePredict
|
||
* Description : pdr卡尔曼滤波器的状态预测方程
|
||
* Date : 2020/7/22 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKFStatePredict(EKFPara_t *kf, double stepLen, PDR_t* g_pdr, int step) {
|
||
|
||
double (*Phi)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]);
|
||
double (*pvec1)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
|
||
double (*pvec2)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
|
||
|
||
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||
|
||
double deltaHeading = g_pdr->heading - g_pdr->lastHeading;
|
||
double angle = g_pdr->heading;
|
||
|
||
//printf("deltaHeading = %f\n", R2D(deltaHeading));
|
||
//if (fabs(deltaHeading) < D2R(10) && g_pdr->lastHeading != 0.0f) {
|
||
// angle = deltaHeading + kf->p_xk[3];
|
||
//}else {
|
||
// angle = g_pdr->heading;
|
||
//}
|
||
|
||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD) {
|
||
deltaHeading = 0;
|
||
}
|
||
|
||
angle = deltaHeading + kf->p_xk[3];
|
||
|
||
|
||
if (fabs(deltaHeading) > 5.5) {
|
||
//printf("%f %f\n", R2D(g_pdr->heading), R2D(g_pdr->lastHeading));
|
||
//printf("d heading = %f\n", R2D(deltaHeading));
|
||
if (deltaHeading < 0) {
|
||
deltaHeading = deltaHeading + D2R(360);
|
||
}else {
|
||
deltaHeading = deltaHeading - D2R(360);
|
||
}
|
||
deltaHeading = 0;
|
||
angle = deltaHeading + kf->p_xk[3];
|
||
}
|
||
|
||
if(step == 0){
|
||
kf->p_xk[3] = angle;
|
||
}
|
||
|
||
// xk+1 = xk + step * cos(angle) PDR位置更新方程
|
||
// yk+1 = yk + step * sin(angle) PDR位置更新方程
|
||
// sk+1 = sk 步长和上一次相同
|
||
// ak+1 = ak * (angle / ak) 或 ak+1 = ak
|
||
|
||
Phi[0][0] = 1; Phi[1][0] = 0; Phi[2][0] = 0; Phi[3][0] = 0;
|
||
Phi[0][1] = 0; Phi[1][1] = 1; Phi[2][1] = 0; Phi[3][1] = 0;
|
||
Phi[0][2] = cos(angle); Phi[1][2] = sin(angle); Phi[2][2] = 1; Phi[3][2] = 0;
|
||
Phi[0][3] = 0; Phi[1][3] = 0; Phi[2][3] = 0; Phi[3][3] = 1;
|
||
|
||
|
||
for (uchar i = 0; i < step; i++) {
|
||
// logzhan 21/02/06:
|
||
// phi[3][3]是为让kf的预测每次都是最新更新值, 如果phi[3][3]每次都是1，可能在惯性
|
||
// 导航更新过程中，偏航角的预测是固定的，实时性没有那么好，下面if判断可以在一定程度
|
||
// 让预测的值更新，但是效果暂时没有感觉出明显的好
|
||
if (fabs(kf->p_xk[3]) > 0.000001 &&
|
||
fabs(angle / kf->p_xk[3]) < 3)
|
||
{
|
||
Phi[3][3] = angle / kf->p_xk[3];
|
||
}
|
||
|
||
// 卡尔曼状态更新方程
|
||
// p_xk = phi * p_xk
|
||
VecMatMultiply(kf->p_xk, Phi, kf->p_xk);
|
||
|
||
// 卡尔曼更新协方差矩阵 : Pk = FPFt + Q
|
||
MatrixMultiply(Phi, kf->p_pk, pvec1); // F*P
|
||
MatrixTrans(Phi, pvec2); // Ft
|
||
MatrixMultiply(pvec1, pvec2, pvec1); // F*P*Ft
|
||
MatrixAdd(pvec1, kf->q, kf->p_pk); // F*P*Ft + Q
|
||
}
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EKFStateUpdate
|
||
* Description : 扩展卡尔曼滤波器的状态更新方程
|
||
* Date : 2020/7/22 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKFStateUpdate(EKFPara_t *kf, GNSS_t *pgnss, classifer *sys, PDR_t *g_pdr,
|
||
int scene_type) {
|
||
|
||
double lambda = 0;
|
||
|
||
// NxN矩阵定义
|
||
double(*H)[N] = (double(*)[N])&(EkfMatBuf[0][0][0]);
|
||
double(*I)[N] = (double(*)[N])&(EkfMatBuf[1][0][0]);
|
||
double(*pvec1)[N] = (double(*)[N])&(EkfMatBuf[2][0][0]);
|
||
double(*Ht)[N] = (double(*)[N])&(EkfMatBuf[3][0][0]);
|
||
double(*pvec3)[N] = (double(*)[N])&(EkfMatBuf[4][0][0]);
|
||
double(*pvec4)[N] = (double(*)[N])&(EkfMatBuf[5][0][0]);
|
||
double(*pv3)[N] = (double(*)[N])&(EkfMatBuf[6][0][0]);
|
||
|
||
memset(EkfMatBuf, 0, sizeof(double) * 7 * N * N);
|
||
memset(EkfVecBuf, 0, sizeof(double) * 5 * N);
|
||
|
||
// Nx1向量定义
|
||
double* z = &(EkfVecBuf[0][0]);
|
||
double(*pvec5) = &(EkfVecBuf[1][0]);
|
||
double(*pvec6) = &(EkfVecBuf[2][0]);
|
||
double(*pv1) = &(EkfVecBuf[3][0]);
|
||
double(*pv2) = &(EkfVecBuf[4][0]);
|
||
|
||
// 创建单位矩阵
|
||
for (char i = 0; i < 4; i++) {
|
||
H[i][i] = 1;
|
||
I[i][i] = 1;
|
||
}
|
||
|
||
// 获取观测向量
|
||
z[0] = pgnss->xNed; // 北向位置
|
||
z[1] = pgnss->yNed; // 东向位置
|
||
z[2] = 0.5; // 步长默认0.5
|
||
|
||
// 这个if条件实际是使用GPS的位置变化量和步数变化量估计步长, 这部分代码还没
|
||
// 调好，实际类似于步长不变
|
||
//if (pgnss->error > 0 && pgnss->error < 3.8 && pgnss->HDOP <= 0.4 &&
|
||
// g_pdr->deltaStepsPerGps != 0 && g_pdr->deltaStepsPerGps < 5) {
|
||
// // 利用GPS速度和步频估计步长
|
||
// //if (g_pdr->motionFreq > 2.5 && (sys->type == 1)) {
|
||
// //
|
||
// // //printf("z[2] = %f\n", z[2]);
|
||
// //}
|
||
// z[2] = pgnss->vel * 0.514 / (g_pdr->deltaStepsPerGps);
|
||
//}else {
|
||
// z[2] = kf->p_xk[2];
|
||
//}
|
||
|
||
z[3] = pgnss->yaw * PI / 180; // 观测的航向角为GPS的航向角
|
||
|
||
// 计算 K = K = PHt / (R + H*P*H^T)
|
||
MatrixTrans(H, Ht); // 计算Ht
|
||
MatrixMultiply(H, kf->p_pk, pvec1); // 计算HP
|
||
MatrixMultiply(pvec1, Ht, pvec3); // pvec3 = H*P*H^T
|
||
MatrixAdd(pvec3, kf->r, pvec4); // pvec4 = R + H*P*H^T
|
||
MatrixInverse(pvec4, pvec1); // 计算 1 / (R + H*P*H^T)
|
||
|
||
for (char i = 0; i < N; i++) {
|
||
for (uchar j = 0; j < N; j++) {
|
||
pv3[i][j] = pvec3[i][j];
|
||
}
|
||
}
|
||
MatrixMultiply(kf->p_pk, Ht, pvec3); // 计算PHt
|
||
MatrixMultiply(pvec3, pvec1, kf->Kk); // 计算增益K = PHt / (R + H*P*H^T)
|
||
|
||
|
||
VecMatMultiply(kf->p_xk, H, pvec5); // pvec5 = Hx'
|
||
VectorSub(z, pvec5, pvec6); // pvec6 = Z - Hx'
|
||
modAngle(&pvec6[3], -PI, PI);
|
||
for (char i = 0; i < N; i++) {
|
||
pv1[i] = pvec6[i];
|
||
}
|
||
|
||
VecMatMultiply(pvec6, kf->Kk, pvec5); // pvec5 = K*( z - Hx')
|
||
// Get the optimally updated state estimation
|
||
VectorAdd(kf->p_xk, pvec5, kf->xk);
|
||
|
||
// ---- prepare the matrix for updated estimate covariance ------
|
||
// pvec1 = K*H
|
||
MatrixMultiply(kf->Kk, H, pvec1);
|
||
// pvec2 = (I - K*H)
|
||
MatrixSub(I, pvec1, Ht);
|
||
// pvec3 = (I - K*H)*P
|
||
MatrixMultiply(Ht, kf->p_pk, pvec3);
|
||
// pvec4 = (I- K*H)^T
|
||
MatrixTrans(Ht, pvec4);
|
||
// pvec1 = (I - K*H)*P*(I- K*H)^T
|
||
MatrixMultiply(pvec3, pvec4, pvec1);
|
||
|
||
// pvec2 = K*R
|
||
MatrixMultiply(kf->Kk, kf->r, Ht);
|
||
// pvec3 = K^T
|
||
MatrixTrans(kf->Kk, pvec3);
|
||
// pvec4 = K*R*K^T
|
||
MatrixMultiply(Ht, pvec3, pvec4);
|
||
// Get the updated estimate covariance: P' = (I - K*H)*P*(I- K*H)^T + K*R*K^T
|
||
MatrixAdd(pvec1, pvec4, kf->pk);
|
||
|
||
// pv2 = (z - Hx')*( H*P*H^T )
|
||
VecMatMultiply(pv1, pv3, pv2);
|
||
|
||
// inner product the pre-fit residual and the transitioned residual: lamda = (z - Hx')*( H*P*H^T ) .* (z-Hx')
|
||
// Calculate the difference between original differenc and the transitioned,
|
||
// if the value is close to zero, it shows that there is a big gap in the
|
||
// oprimization. Thus, we believe our PDR algorithm
|
||
for (char i = 0; i < N; i++) {
|
||
lambda += pv2[i] * pv1[i];
|
||
}
|
||
|
||
kf->lambda = lambda;
|
||
kf->plat = pgnss->lat;
|
||
kf->plon = pgnss->lon;
|
||
|
||
if (lambda >= 200)return;
|
||
|
||
for (char i = 0; i < N; i++) {
|
||
kf->xk[i] = kf->p_xk[i];
|
||
for (int j = 0; j < N; j++) {
|
||
kf->pk[i][j] = kf->p_pk[i][j];
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
void calStepLastTime(StepPredict *stepPredict, double timestamp, unsigned long steps_last,
|
||
unsigned long steps)
|
||
{
|
||
float tmpTime = 0;
|
||
if (stepPredict->lastTime > 0 && stepPredict->deltaStep == 0 && (steps - steps_last) > 0)
|
||
{
|
||
tmpTime = (float)((timestamp - stepPredict->lastTime) / (steps - steps_last));
|
||
if (tmpTime > 300 && tmpTime < 800)
|
||
{
|
||
if (stepPredict->meanTime > 0){
|
||
stepPredict->meanTime = stepPredict->meanTime * 0.5f + tmpTime * 0.5f;
|
||
}else{
|
||
stepPredict->meanTime = tmpTime;
|
||
}
|
||
}
|
||
}
|
||
|
||
stepPredict->fnum = 0;
|
||
stepPredict->fsum = 0;
|
||
|
||
stepPredict->lastTime = timestamp;
|
||
stepPredict->deltaStep = 0;
|
||
|
||
}
|
||
|
||
int predictStep(StepPredict *stepPredict, double timestamp, unsigned long steps_last, float gnssVel)
|
||
{
|
||
int step = 0;
|
||
float mean_time = 400;
|
||
double dis = 0;
|
||
|
||
if (stepPredict->meanTime > 0)
|
||
mean_time = stepPredict->meanTime;
|
||
|
||
if ((steps_last > 0) && (stepPredict->fnum > 0) && (timestamp - stepPredict->lastTime > mean_time * 3) && (stepPredict->lastTime > 0))
|
||
{
|
||
stepPredict->fsum = stepPredict->fsum / stepPredict->fnum;
|
||
dis = sqrt(pow(GpsPos[HIST_GPS_NUM - 1][0]- GpsPos[HIST_GPS_NUM - 2][0], 2) + pow(GpsPos[HIST_GPS_NUM - 1][1] - GpsPos[HIST_GPS_NUM - 2][1], 2)
|
||
+ pow(GpsPos[HIST_GPS_NUM - 1][2] - GpsPos[HIST_GPS_NUM - 2][2], 2));
|
||
|
||
if (stepPredict->fsum != 0.0 && (gnssVel > 1.0 || (dis > 1.0 && dis < 3.0)))
|
||
{
|
||
step = (int)((timestamp - stepPredict->lastTime) / mean_time);
|
||
stepPredict->deltaStep += step;
|
||
}
|
||
|
||
stepPredict->lastTime = timestamp;
|
||
stepPredict->fnum = 0;
|
||
stepPredict->fsum = 0;
|
||
}
|
||
|
||
return step;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : StateRecognition
|
||
* Description : 根据加速度及判断当前状态是否平稳 1: 不平稳 0: 平稳
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void StateRecognition(float *acc, classifer *sys) {
|
||
|
||
}
|
||
|
||
|
||
double CalGnssTrackHeading(double gpsPos[][3], GNSS_t pgnss)
|
||
{
|
||
int i;
|
||
double lla[3] = { 0.0 };
|
||
double ned1[3] = { 0.0 };
|
||
double temp[2][HIST_GPS_NUM] = { { 0.0 } };
|
||
double angle[HIST_GPS_NUM - 1] = { 0.0 };
|
||
|
||
double ned2[3] = { 0.0 };
|
||
double maxn = 0;
|
||
double maxe = 0;
|
||
double minn = 0;
|
||
double mine = 0;
|
||
double meanx = 0;
|
||
double meany = 0;
|
||
|
||
float error = 0;
|
||
float sst = 0;
|
||
float r2 = 0;
|
||
|
||
double range = 0;
|
||
double yaw = 0;
|
||
double tempYaw = 0;
|
||
double diffAngle = 0;
|
||
|
||
double para[3] = { 0 };
|
||
int flg = 0;
|
||
|
||
lla[0] = pgnss.lat;
|
||
lla[1] = pgnss.lon;
|
||
|
||
for (i = 0; i < HIST_GPS_NUM - 1; i++)
|
||
{
|
||
gpsPos[i][0] = gpsPos[i + 1][0];
|
||
gpsPos[i][1] = gpsPos[i + 1][1];
|
||
gpsPos[i][2] = gpsPos[i + 1][2];
|
||
}
|
||
|
||
if ((lla[0] > 0) && (lla[1] > 0))
|
||
{
|
||
//经纬度WGS84转地心地固坐标系ECEF
|
||
WGS842ECEF(lla, gpsPos[HIST_GPS_NUM - 1]);
|
||
}
|
||
else
|
||
{
|
||
gpsPos[HIST_GPS_NUM - 1][0] = 0;
|
||
gpsPos[HIST_GPS_NUM - 1][1] = 0;
|
||
gpsPos[HIST_GPS_NUM - 1][2] = 0;
|
||
return -1;
|
||
}
|
||
if (pgnss.error > 40)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
for (i = 0; i < HIST_GPS_NUM - 1; i++)
|
||
{
|
||
if ((fabs(gpsPos[i][0]) < 1e-6) && (fabs(gpsPos[i][1]) < 1e-6) && (fabs(gpsPos[i][2]) < 1e-6))
|
||
{
|
||
return -1;
|
||
}
|
||
}
|
||
//地心地固坐标系ECEF转东北天坐标系NED
|
||
ECEF2NED(gpsPos[HIST_GPS_NUM - 1], lla, ned1);
|
||
|
||
for (i = HIST_GPS_NUM - 2; i >= 0; i--) {
|
||
|
||
ECEF2NED(gpsPos[i], lla, ned2);
|
||
|
||
temp[0][i] = ned1[0] - ned2[0];
|
||
temp[1][i] = ned1[1] - ned2[1];
|
||
|
||
meanx += temp[0][i] / HIST_GPS_NUM;
|
||
meany += temp[1][i] / HIST_GPS_NUM;
|
||
|
||
if (maxn > temp[0][i])
|
||
{
|
||
maxn = temp[0][i];
|
||
}
|
||
if (minn < temp[0][i])
|
||
{
|
||
minn = temp[0][i];
|
||
}
|
||
if (maxe > temp[1][i])
|
||
{
|
||
maxe = temp[1][i];
|
||
}
|
||
if (mine < temp[1][i])
|
||
{
|
||
mine = temp[1][i];
|
||
}
|
||
|
||
angle[i] = atan2(temp[1][i], temp[0][i]);
|
||
if (angle[i] < 0)
|
||
angle[i] += TWO_PI;
|
||
}
|
||
flg = leastDistanceLinearFit(temp[0], temp[1], HIST_GPS_NUM, para);
|
||
if (flg < 0)
|
||
return -1;
|
||
|
||
for (i = HIST_GPS_NUM - 1; i >= 0; i--) {
|
||
error += (float)(vivopow(para[0] * temp[0][i] + para[1] * temp[1][i] + para[2], 2) / (vivopow(para[0], 2) + vivopow(para[1], 2)));
|
||
sst += (float)(vivopow(meanx - temp[0][i], 2) + vivopow(meany - temp[1][i], 2));
|
||
}
|
||
|
||
if (sst > 0)
|
||
{
|
||
r2 = 1 - error / sst;
|
||
}
|
||
range = sqrt(vivopow(maxn - minn, 2) + vivopow(maxe - mine, 2));
|
||
//printf("%f,%f,%f\n", r2, error, range);
|
||
if (r2 < 0.95 || range < 1.5 || error > 3.0)
|
||
return -1;
|
||
|
||
yaw = atan(-para[0] / para[1]);
|
||
modAngle(&yaw, 0, TWO_PI);
|
||
|
||
tempYaw = meanAngle(angle, HIST_GPS_NUM - 1);
|
||
modAngle(&tempYaw, 0, TWO_PI);
|
||
|
||
diffAngle = tempYaw - yaw;
|
||
modAngle(&diffAngle, -PI, PI);
|
||
|
||
if (fabs(diffAngle) > PI / 2)
|
||
yaw += PI;
|
||
modAngle(&yaw, 0, TWO_PI);
|
||
|
||
return yaw;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : InsLocationPredict
|
||
* Description : PDR惯导位置更新,
|
||
* Date : 2020/07/08 logzhan
|
||
* 2021/02/06 logzhan : 感觉计步器输出不够平均，有时候
|
||
* 会造成PDR前后位置不够均匀
|
||
*---------------------------------------------------------------------**/
|
||
void InsLocationPredict(PDR_t *g_pdr, StepPredict *stepPredict, IMU_t *ss_data,
|
||
GNSS_t *pgnss, EKFPara_t *kf) {
|
||
int step;
|
||
long deltaStep;
|
||
double stepLength = 0.7;
|
||
|
||
stepPredict->fnum++;
|
||
stepPredict->fsum += g_pdr->motionFreq;
|
||
|
||
ulong steps = g_pdr->steps;
|
||
ulong lastSteps = g_pdr->lastSteps;
|
||
|
||
if (steps - lastSteps > 0 && g_pdr->motionFreq != 0.0) {
|
||
deltaStep = steps - lastSteps - stepPredict->deltaStep;
|
||
|
||
// 应该是防止计步器异常情况，一次计步特别多
|
||
if (deltaStep > 5)deltaStep = 5;
|
||
|
||
if (stepPredict->meanTime > 0)
|
||
{
|
||
step = (int)((ss_data->gyr.time - stepPredict->lastTime) / stepPredict->meanTime + 0.5);
|
||
|
||
if (step > deltaStep && step <= 3)deltaStep = step;
|
||
}
|
||
if (lastSteps == 0)deltaStep = 0;
|
||
|
||
if (g_pdr->heading > 0) {
|
||
EKFStatePredict(kf, stepLength, g_pdr, deltaStep);
|
||
g_pdr->fusionPdrFlg = PDR_TRUE;
|
||
}
|
||
calStepLastTime(stepPredict, ss_data->gyr.time, lastSteps, steps);
|
||
}else{
|
||
// 有时候，计步器可能考虑到稳定性暂时不更新步数，因此需要利用GNSS速度预测步数，达到
|
||
// 惯导更新位置的目的
|
||
int preStep = predictStep(stepPredict, ss_data->gyr.time, lastSteps, pgnss->vel);
|
||
if (g_pdr->heading > 0){
|
||
EKFStatePredict(kf, stepLength, g_pdr, preStep);
|
||
if(preStep > 0)g_pdr->fusionPdrFlg = PDR_TRUE;
|
||
}
|
||
}
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : ResetOutputResult
|
||
* Description : 1、初始化解算初始位置、卡尔曼滤波初始参数
|
||
* 2、GPS值赋值给result
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
int ResetOutputResult(GNSS_t *pgnss, PDR_t* g_pdr, EKFPara_t *kf, lct_fs *result) {
|
||
|
||
double stepLength = 0.7;
|
||
double ned[3] = { 0 };
|
||
|
||
g_pdr->pllaInit[0] = pgnss->lat;
|
||
g_pdr->pllaInit[1] = pgnss->lon;
|
||
g_pdr->pllaInit[2] = 0;
|
||
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
|
||
|
||
g_pdr->x0 = ned[0];
|
||
g_pdr->y0 = ned[1];
|
||
|
||
kf->p_xk[0] = ned[0];
|
||
kf->p_xk[1] = ned[1];
|
||
kf->p_xk[2] = stepLength;
|
||
// 重置输出GPS时，GPS的航向角不一定准
|
||
kf->p_xk[3] = pgnss->yaw*RADIAN_PER_DEG;
|
||
|
||
for (uchar i = 0; i < N; i++) {
|
||
kf->xk[i] = kf->p_xk[i];
|
||
}
|
||
// 清除卡尔曼Q矩阵和R矩阵
|
||
for (int i = 0; i < N; i++) {
|
||
for (int l = 0; l < N; l++) {
|
||
kf->q[i][l] = 0.0;
|
||
kf->r[i][l] = 0.0;
|
||
}
|
||
}
|
||
// 输出GPS位置结果
|
||
OutputOriginGnssPos(pgnss, result);
|
||
g_pdr->NoGnssCount = 0;
|
||
return PDR_TRUE;
|
||
}
|
||
|
||
int InitProc(GNSS_t *pgnss, EKFPara_t *kf, PDR_t* g_pdr,lct_fs *result)
|
||
{
|
||
double stepLen = 0.7; // 初始运动步长为0.7m
|
||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel >= 1.5 && pgnss->yaw != -1) {
|
||
// plla 类似位置坐标的原点(单位是经纬度)
|
||
|
||
g_pdr->pllaInit[0] = pgnss->lat;
|
||
g_pdr->pllaInit[1] = pgnss->lon;
|
||
g_pdr->pllaInit[2] = 0;
|
||
|
||
// plla 转 ned
|
||
double ned[3] = { 0 };
|
||
WGS842NED(g_pdr->pllaInit, g_pdr->pllaInit, ned);
|
||
|
||
// 在NED坐标系的计算, 单位是米
|
||
g_pdr->x0 = ned[0];
|
||
g_pdr->y0 = ned[1];
|
||
|
||
// 初始化卡尔曼滤波器的观测量
|
||
kf->p_xk[0] = ned[0]; // 状态量1: 北向x
|
||
kf->p_xk[1] = ned[1]; // 状态量2：东向y
|
||
kf->p_xk[2] = stepLen; // 状态量3：步长
|
||
kf->p_xk[3] = pgnss->yaw * RADIAN_PER_DEG; // 状态量4：航向角
|
||
|
||
for (uchar i = 0; i < N; i++) {
|
||
kf->xk[i] = kf->p_xk[i];
|
||
}
|
||
|
||
kf->initHeading = kf->p_xk[3];
|
||
for (int i = 0; i < N; i++) {
|
||
for (int l = 0; l < N; l++) {
|
||
kf->q[i][l] = 0.0;
|
||
kf->r[i][l] = 0.0;
|
||
}
|
||
}
|
||
OutputOriginGnssPos(pgnss, result);
|
||
g_pdr->sysStatus = IS_NORMAL;
|
||
|
||
return PDR_TRUE;
|
||
}
|
||
|
||
if (pgnss->satellites_num > 4 && pgnss->HDOP < 2.0 && pgnss->vel > 0 && pgnss->yaw != -1) {
|
||
OutputOriginGnssPos(pgnss, result);
|
||
return PDR_TRUE;
|
||
}
|
||
return PDR_FALSE;
|
||
}
|
||
|
||
/**----------------------------------------------------------------------
|
||
* Function : EKFCalQRMatrix
|
||
* Description : 根据GPS信号特征调整卡尔曼滤波噪声矩阵，q矩阵是过程噪声矩阵，
|
||
* 需要跟惯导预测位置精度相关。r矩阵为GNSS观测噪声，跟GPS输出的
|
||
* 信息精度有关。
|
||
* Date : 2022/09/19 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void EKFCalQRMatrix(lct_nmea *nmea, PDR_t *g_pdr, classifer *sys,
|
||
EKFPara_t *kf) {
|
||
|
||
if (g_pdr->motionType != PDR_MOTION_TYPE_HANDHELD &&
|
||
g_pdr->motionType != PDR_MOTION_TYPE_STATIC) {
|
||
kf->q[0][0] = 25; // PDR预测位置噪声
|
||
kf->q[1][1] = 25; // PDR预测位置噪声
|
||
kf->q[2][2] = 1; // PDR步长噪声
|
||
kf->q[3][3] = 100; // PDR方向噪声
|
||
|
||
kf->r[0][0] = 50; // GPS观测位置噪声
|
||
kf->r[1][1] = 50; // GPS观测位置噪声
|
||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||
kf->r[3][3] = 50; // GPS方向噪声
|
||
return;
|
||
}
|
||
|
||
kf->q[0][0] = 0.1; // PDR预测位置噪声
|
||
kf->q[1][1] = 0.1; // PDR预测位置噪声
|
||
kf->q[2][2] = 1; // PDR步长噪声
|
||
kf->q[3][3] = 1; // PDR方向噪声
|
||
|
||
kf->r[0][0] = 100; // GPS预测位置噪声
|
||
kf->r[1][1] = 100; // GPS预测位置噪声
|
||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||
kf->r[3][3] = 1000; // GPS方向噪声
|
||
|
||
if (nmea->accuracy.err > 0 &&
|
||
nmea->accuracy.err < 3.80 &&
|
||
nmea->gga.hdop <= 0.4)
|
||
{
|
||
kf->r[0][0] = 50; // GPS观测位置噪声
|
||
kf->r[1][1] = 50; // GPS观测位置噪声
|
||
kf->r[2][2] = 0.1; // GPS步长噪声
|
||
kf->r[3][3] = 50; // GPS方向噪声
|
||
}
|
||
|
||
if (fabs(R2D((g_pdr->gpsHeading - g_pdr->lastGpsHeading))) > 50 ||
|
||
g_pdr->gpsSpeed < 0.7 || g_pdr->gpsHeading == -1) {
|
||
kf->r[3][3] = 10000;
|
||
}
|
||
}
|
||
/**----------------------------------------------------------------------
|
||
* Function : GnssInsLocFusion
|
||
* Description : PDR的GNSS和INS融合定位
|
||
* Date : 2022/09/21 logzhan
|
||
*---------------------------------------------------------------------**/
|
||
void GnssInsLocFusion(GNSS_t *pgnss, PDR_t *g_pdr, classifer *sys, double yaw_bias,
|
||
EKFPara_t *kf, lct_fs *res) {
|
||
double plla[3] = { 0 };
|
||
double ned[3] = { 0 };
|
||
double yaw_thr = 6;
|
||
|
||
int scene_type = g_pdr->sceneType;
|
||
|
||
plla[0] = pgnss->lat;
|
||
plla[1] = pgnss->lon;
|
||
plla[2] = 0;
|
||
|
||
WGS842NED(plla, g_pdr->pllaInit, ned);
|
||
|
||
pgnss->xNed = ned[0];
|
||
pgnss->yNed = ned[1];
|
||
|
||
//调整融合策略
|
||
EKFStateUpdate(kf, pgnss, sys, g_pdr, scene_type);
|
||
|
||
for (uchar i = 0; i < N; i++) {
|
||
for (uchar l = 0; l < N; l++) {
|
||
kf->p_pk[i][l] = kf->pk[i][l];
|
||
}
|
||
kf->p_xk[i] = kf->xk[i];
|
||
}
|
||
|
||
ned[0] = kf -> p_xk[0] - g_pdr->x0;
|
||
ned[1] = kf -> p_xk[1] - g_pdr->y0;
|
||
ned[2] = 0;
|
||
|
||
NED2WGS84(g_pdr->pllaInit, ned, plla);
|
||
|
||
res->latitude = plla[0];
|
||
res->latitude_ns = pgnss->lat_ns;
|
||
res->longitudinal = plla[1];
|
||
res->longitudinal_ew = pgnss->lon_ew;
|
||
res->time = pgnss->timestamps;
|
||
|
||
g_pdr->NoGnssCount = 0;
|
||
}
|