Skip to content

GitLab

Commit 9cf62671 authored by 施垒's avatar 施垒 👍
Browse files

[#9]启迪项目第一阶段代码，优化了IMU静态判断系数，实时静态检测，卫星定位精度判断，输出频率控制

parent b33498c3
Shi.l_9_启迪项目第二阶段算法优化
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 95 additions and 117 deletions
+95 -117
KFApp.cpp
View file @ 9cf62671
......@@ -21,15 +21,15 @@ void CKFApp::Init16(const CSINS &sins0) // MEMS
200.0*glv.dph, 200.0*glv.dph, 200.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg);
//Pk.SetDiag2(10 * glv.deg, 10 * glv.deg, 30 * glv.deg, 1.0, 1.0, 1.0, 100.0 / glv.Re, 100.0 / glv.Re, 100.0,
// 1000.0*glv.dph, 1000.0*glv.dph, 1000.0*glv.dph, 10.0*glv.mg, 10.0*glv.mg, 10.0*glv.mg);
// 系统噪声Qt: old is 1.0 100.0
Qt.Set2(500*glv.dpsh, 500*glv.dpsh, 500*glv.dpsh, 16000*glv.ugpsHz,16000*glv.ugpsHz, 16000*glv.ugpsHz, 0.0,0.0,0.0,
// 系统噪声Qt: old is 1.0*glv.dpsh; 100.0*glv.ugpsHz
Qt.Set2(10*glv.dpsh, 10*glv.dpsh, 10*glv.dpsh, 500*glv.ugpsHz,5000*glv.ugpsHz, 5000*glv.ugpsHz, 0.0,0.0,0.0,
0.0,0.0,0.0, 0.0,0.0,0.0);
//量测噪声 8.12 改 old is 0.5 10m
Rt.Set2(0.1, 0.1 , 0.1 , 3 /glv.Re, 3 /glv.Re,5);//该参数为单点解时候的定位误差参数，对异常量测有一定鲁棒性
Rmax = Rt*100; Rmin = Rt*0.01; Rb.Set(0.9,0.9,0.9, 0.9,0.9,0.9);
Rt.Set2(0.2, 0.2 , 0.2 , 2 /glv.Re, 2 /glv.Re,5);//该参数为单点解时候的定位误差参数，对异常量测有一定鲁棒性
Rmax = Rt*20; Rmin = Rt*0.01; Rb.Set(0.9,0.9,0.9, 0.9,0.9,0.9);
//Xk反馈量 X看有效判断
FBTau.Set(1.0,1.0,10.0, 1.0,1.0,1.0, 1.0,1.0,1.0, 5.0,5.0,5.0, 5.0,5.0,5.0);//FBTau.Set(1.0,1.0,10.0, 1.0,1.0,1.0, 1.0,1.0,1.0, 10.0,10.0,10.0, 10.0,10.0,10.0);
FBMax.Set(INF,INF,INF, INF,INF,INF, INF,INF,INF, 7200.0*glv.dph,7200.0*glv.dph,7200.0*glv.dph, 50.0*glv.mg,50.0*glv.mg,50.0*glv.mg);
FBMax.Set(INF,INF,INF, INF,INF,INF, INF,INF,INF, 7200.0*glv.dph,7200.0*glv.dph,7200.0*glv.dph, 80.0*glv.mg,80.0*glv.mg,80.0*glv.mg);
//FBMax.Set(INF, INF, INF, INF, INF, INF, INF, INF, INF, 3000.0*glv.dph, 3000.0*glv.dph, 3000.0*glv.dph, 50.0*glv.mg, 50.0*glv.mg, 50.0*glv.mg);//old parameter
}
......
PSINS.cpp
View file @ 9cf62671
......@@ -1268,14 +1268,10 @@ void CSINSKF::Update(const CVect3 *wm, const CVect3 *vm, int nSamples, double ts
{
sins.Update(wm, vm, nSamples, ts);//不可交换误差
TimeUpdate(sins.nts); kftk = sins.tk;//KF的时间更新 主要是X 和P
//if(kftk>370.6)//-1.0&&Xk.dd[0] / glv.deg > -1.05)
//printf("Tsins delta pit%f delta roll %f\r\n", Xk.dd[0]/glv.deg, Xk.dd[1] / glv.deg);
#if WIN32
// vel constrain
#endif
MeasUpdate();// KF的量测更新 X P K
//printf("Msins delta pit%f delta roll %f time %f\r\n", Xk.dd[0] / glv.deg, Xk.dd[1] / glv.deg,kftk);
}
void CSINSKF::Feedback(double fbts)
......
main_sinsgps_test.cpp
View file @ 9cf62671
#include "kfapp.h"
#include "sinsapi.h"
//CKFApp kf;
#define rawdata919 1
//CFileRdWt dir("C:\\F\\SINS-GNSS\\Yan_PSINS_Program\\PSINS-CPP-VC60（shilei）\\PSINS-CPP\\Data\\");
MPU_Data_value mpu_Data_value;
gga_t gga;
vtg_t vtg;
dhv_t dhv;
double gga_delay = 0.0;
double gga_delay = 0.06;//默认设置 gga延迟是60ms
int gps_ok_flag = 0;
bool gpsUpdate = true;
int main(void)
{
CFileRdWt fin("rawdataDemo9.1906.txt", 27);// , fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// realtime 不需要
CFileRdWt fin("rawdataDemo10.2601.txt", 27); // , fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// realtime 不需要
//CFileRdWt fin("rawdata1.txt", 29), fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// 非PSINS
CVect3 wm, vm, eb0, gpsvn, gpspos;
double timu, timu0;
for (int i = 1; i < 20000 * 50; i++)
for (int i = 1; i < 12500 * 50; i++)
{
fin.load(); if (fin.IsEOF()) break;
// 为启迪坐标系错误调整
if (i == 1) timu0 = fin.buff[0];
mpu_Data_value.time = fin.buff[0] - timu0;
#if rawdata919
mpu_Data_value.Gyro[0] = fin.buff[2];// dps // 启迪项目保存的rawdata 坐标系统一在ENU 单位也已标准化
mpu_Data_value.Gyro[1] = fin.buff[1];
mpu_Data_value.Gyro[2] = -fin.buff[3];
mpu_Data_value.Accel[0] = fin.buff[5];// m/s2
mpu_Data_value.Accel[1] = fin.buff[4];
mpu_Data_value.Accel[2] = -fin.buff[6];
#else
mpu_Data_value.Gyro[0] = -fin.buff[1];// dps // 启迪项目保存的rawdata 坐标系统一在ENU 单位也已标准化
mpu_Data_value.Gyro[1] = fin.buff[2];//QIDI 923.01 数据
mpu_Data_value.Gyro[2] = -fin.buff[3];
mpu_Data_value.Accel[0] = -fin.buff[4];// m/s2
mpu_Data_value.Gyro[0] = fin.buff[1]; // dps // 启迪项目保存的rawdata 坐标系统一在ENU 单位也已标准化
mpu_Data_value.Gyro[1] = fin.buff[2];
mpu_Data_value.Gyro[2] = fin.buff[3];
mpu_Data_value.Accel[0] = fin.buff[4]; // m/s2
mpu_Data_value.Accel[1] = fin.buff[5];
mpu_Data_value.Accel[2] = -fin.buff[6];
#endif
mpu_Data_value.Temp = fin.buff[26];//imu temprature
mpu_Data_value.Accel[2] = fin.buff[6];
mpu_Data_value.Temp = fin.buff[26]; //imu temprature
timu = mpu_Data_value.time;
//19开始是定位纬度整数
if (fin.buff[20] == 0&& fin.buff[19] == 0)
......@@ -51,13 +42,7 @@ int main(void)
}
if (gpsUpdate)//纬度整数位,以及小数位比较
{
//vtg.cogt = atan2(fin.buff[16], fin.buff[17])/glv.deg;//deg 注意PSINS里边的航向是北偏西为正+-pi
//(vtg.cogt > 0.0)? vtg.cogt:(vtg.cogt + 360.0);
//vtg.kph = norm(*(CVect3*)&fin.buff[16])*3.6;
//vtg.cogm = fin.buff[16];//venu
//vtg.knots = fin.buff[17];
//vtg.kph = fin.buff[18];
dhv.vel_e = fin.buff[16];//venu
dhv.vel_e = fin.buff[16];//venu //注意PSINS里边的航向是北偏西为正+-pi
dhv.vel_n = fin.buff[17];
dhv.vel_u = fin.buff[18];
......@@ -66,15 +51,11 @@ int main(void)
gga.lon_d =int( fin.buff[20]);//开发板数据保存成了2位
gga.lon_m = (fin.buff[20] - gga.lon_d)*60;
gga.altitude = fin.buff[21];
#if rawdata916
gga.h_DOP = 1.0;
#else
gga.h_DOP = fin.buff[22];
#endif
//gga_delay= fin.buff[23];
gga_delay= fin.buff[23];
gga.num_satellite = fin.buff[24];
gga.fix = fin.buff[25];
//温度= fin.buff[26];
gps_ok_flag = 1;
}
else
......
sinsapi.cpp
View file @ 9cf62671
......@@ -7,22 +7,26 @@
#include "KFApp.h"
#include "sinsapi.h"
#define FreqOut 50 //qidi
#ifdef WIN32
#define FreqOut 50
#else
#define Freq_imu 1 //qidi 设备输出频率
#endif
#define Freq_imu 50 //qidi 50Hz imu,
#define Freq_imu2 0.5*Freq_imu //50%*Freq_imu
#define SaveRawdata 0
#define INS_time 15 //纯惯导时间长度
#define Reget_satelite 4//重获卫星2s 开始显示融合定位
#define Std_static_acc0 0.25//绝对静止qidi
#define Reget_satelite 6//重获卫星6s 开始显示融合定位
#define Std_static_acc0 0.2//绝对静止qidi
#define Std_static_acc2 0.3//宽松静止条件
#define Std_static_gyr0 0.12//绝对静止
#define Std_static_gyr2 0.2//宽松静止条件
#define Std_static_gyr2 0.25//宽松静止条件
#define Tempreture_comp 0 // 温度补偿开关
// 822
#ifdef WIN32
CFileRdWt dir("C:\\F\\SINS-GNSS\\Yan_PSINS_Program\\PSINS-CPP-VC60(qidi)\\PSINS-CPP\\Data\\");
CFileRdWt dir("C:\\F\\SINS-GNSS\\Yan_PSINS_Program\\PSINS-CPP-VC60(qidi108)\\PSINS-CPP\\Data\\");
CFileRdWt fins("ins.bin", 0), fkf("kf.bin", 0), frk("rk.bin", 0);// realtime 不需要
#endif
......@@ -32,6 +36,7 @@ int OutCount = 1;//
double Ts = 1.0 / Freq_imu;
double ggatime = 0.0;
double timu_m = 0.0;
double last_timu_m = 0;
bool KfInit = false;
bool count1KfInit = true;
bool install_init = false;
......@@ -131,10 +136,10 @@ Out_Data* SINS_API(MPU_Data_value *mpu_Data_value, gga_t *gga, vtg_t *vtg, dhv_t
{
imuavr.stdimu[0] = GetAveStdRMS(imuavr.ax, Freq_imu, 1);
imuavr.stdimu[1] = GetAveStdRMS(imuavr.ay, Freq_imu, 1);
imuavr.stdimu[2] = GetAveStdRMS(imuavr.az, Freq_imu, 1);
imuavr.stdimu[2] = GetAveStdRMS(imuavr.gx, Freq_imu, 1);
imuavr.stdimu[3] = GetAveStdRMS(imuavr.gz, Freq_imu, 1);
imu_cnt = 0;
if (imuavr.stdimu[0] < Std_static_acc2&&imuavr.stdimu[1] < Std_static_acc2&&imuavr.stdimu[3] < Std_static_gyr2)//大致静止
if (imuavr.stdimu[0] < Std_static_acc2&&imuavr.stdimu[1] < Std_static_acc2&&imuavr.stdimu[2] < Std_static_gyr2&&imuavr.stdimu[3] < Std_static_gyr2)//大致静止
{
if (!imu_instal)
{
......@@ -152,48 +157,47 @@ Out_Data* SINS_API(MPU_Data_value *mpu_Data_value, gga_t *gga, vtg_t *vtg, dhv_t
imuavr.att_install.i = atan(imuavr.install_acc[1] / imuavr.install_acc[2]);//pitch ,rad
imuavr.att_install.k = 0.0;
imu_instal = true;//安装误差初始化判断
//printf(" qidi install ok \r\n");
}
imu_static = true;//静态判断
#if 1//实时静态判断
if (norm(gpsvn) < 3.0&&imuavr.stdimu[0] < Std_static_acc0&&imuavr.stdimu[1] < Std_static_acc0&&imuavr.stdimu[2] < Std_static_gyr0&&imuavr.stdimu[3] < Std_static_gyr0)//imu ax ay gx gz 绝对静止
{
eb1.i = GetAveStdRMS(imuavr.gx, Freq_imu, 0);//bias均值
eb1.j = GetAveStdRMS(imuavr.gy, Freq_imu, 0);
eb1.k = GetAveStdRMS(imuavr.gz, Freq_imu, 0);
eb0.i = eb1.i - kf.sins.eb.i / glv.deg;
eb0.j = eb1.j - kf.sins.eb.j / glv.deg;
eb0.k = eb1.k - kf.sins.eb.k / glv.deg;
imuavr.install_acc[0] = GetAveStdRMS(imuavr.ax, 50, 0) - kf.sins.db.i;
imuavr.install_acc[1] = GetAveStdRMS(imuavr.ay, 50, 0) - kf.sins.db.j;
imuavr.install_acc[2] = GetAveStdRMS(imuavr.az, 50, 0) - kf.sins.db.k;
double gyz = sqrt(imuavr.install_acc[1] * imuavr.install_acc[1] + imuavr.install_acc[2] * imuavr.install_acc[2]);
//计算 install roll pitch
kf.sins.vn = 0.0;//速度都置零
att_static.j = -1.0*atan(imuavr.install_acc[0] / gyz) - imuavr.att_install.j;//roll ,rad
att_static.i = atan(imuavr.install_acc[1] / imuavr.install_acc[2]) - imuavr.att_install.i;//pitch ,rad
att_static.k = kf.sins.att.k;
kf.sins.qnb = a2qua(att_static);//静态时候的姿态校正
#if WIN32 // 验证该姿态强制约束是不是正确
CMat3 Cnb_install, Cnb_install0;
CVect3 acc, vmm;
Cnb_install = a2mat(att_static);
acc = *(CVect3*)imuavr.install_acc;
vmm = Cnb_install * acc;
//printf("dele 1 %f %f %f \r\n", vmm.i, vmm.j, vmm.k);
Cnb_install0 = a2mat(imuavr.att_install);
vmm = Cnb_install0 * vmm;// 加速度m/s2
#endif
}//
#endif
}
else
{
imu_static = false;
}
#if 0
//if (imuavr.stdimu[0] < Std_static_acc0&&imuavr.stdimu[1] < Std_static_acc0&&imuavr.stdimu[3] < Std_static_gyr0)//imu ax ay az gz 绝对静止
{
//eb1.i = GetAveStdRMS(imuavr.gx, Freq_imu, 0);//bias均值
//eb1.j = GetAveStdRMS(imuavr.gy, Freq_imu, 0);
//eb1.k = GetAveStdRMS(imuavr.gz, Freq_imu, 0);
////if (timu_m>304)
{
//eb0.i = eb1.i - kf.sins.eb.i / glv.deg;
//eb0.j = eb1.j - kf.sins.eb.j / glv.deg;
//eb0.k = eb1.k - kf.sins.eb.k / glv.deg;
//imuavr.install_acc[0] = GetAveStdRMS(imuavr.ax, 50, 0) - kf.sins.db.i;
//imuavr.install_acc[1] = GetAveStdRMS(imuavr.ay, 50, 0) - kf.sins.db.j;
//imuavr.install_acc[2] = GetAveStdRMS(imuavr.az, 50, 0) - kf.sins.db.k;
//double gyz = sqrt(imuavr.install_acc[1] * imuavr.install_acc[1] + imuavr.install_acc[2] * imuavr.install_acc[2]);
//此处 可以考虑优化重力加速度系数适应南北方重力系数差异
//计算 install roll pitch
//kf.sins.vn = 0.0;//速度都置零
//att_static.j = -1.0*atan(imuavr.install_acc[0] / gyz) - imuavr.att_install.j;//roll ,rad
//att_static.i = atan(imuavr.install_acc[1] / imuavr.install_acc[2]) - imuavr.att_install.i;//pitch ,rad
//att_static.k = kf.sins.att.k;
//kf.sins.qnb = a2qua(att_static);//静态时候的姿态校正
//CMat3 Cnb_install;
////Cnb_install = a2mat(imuavr.att_install);
//Cnb_install = a2mat(att_static);
//CVect3 acc, vmm;
//acc = *(CVect3*)imuavr.install_acc;
//vmm = Cnb_install * acc;
}
}//==25
#endif
}//imu install angle和gyro eb
wm = wm * glv.dps*Ts; vm = vm * Ts;//增量化 单位 rad和 m/s
......@@ -210,58 +214,56 @@ Out_Data* SINS_API(MPU_Data_value *mpu_Data_value, gga_t *gga, vtg_t *vtg, dhv_t
gpsvn.j = dhv->vel_n;//N向速度 m/s
gpsvn.k = dhv->vel_u;//U向速度
if (count1KfInit && norm(gpsvn) > 3.0&& gga->num_satellite > 4 && gga->h_DOP < 5.0)// KF 初始化条件
if (count1KfInit && norm(gpsvn) > 3.0&& gga->num_satellite > 9 && gga->h_DOP < 0.9)// KF 初始化条件
{
double yaw0 = 0.0;
yaw0 = -atan2(gpsvn.i, gpsvn.j) / glv.deg;//deg
kf.Init16(CSINS(a2qua(CVect3(0.0, 0.0, yaw0)*glv.deg), gpsvn, gpspos, timu_m)); // 初始化姿态方位和位置 0.0是时间
kf.Init16(CSINS(a2qua(CVect3(0.0, 0.0, yaw0)*glv.deg), gpsvn, gpspos, timu_m)); // 初始化姿态方位和位置 0.0是时间
KfInit = true;
count1KfInit = false;
//printf("sins kf.Init16 imutime %.3lf\r\n", mpu_Data_value->time);
last_timu_m = timu_m;
#if WIN32
printf("sins kf.Init16 imutime %.3lf\r\n", timu_m);
#endif
}
if (KfInit&& gga->num_satellite > 5 && gga->h_DOP < 5.0)
//Measurement Update
if (KfInit && gga->num_satellite > 9 && gga->h_DOP < 0.9)//&&delta_pos<20.0
{
//if (gga->fix == 4 || gga->fix == 5) // 降低R的值
kf.SetMeas(&gpsvn, &gpspos, gga_delay);// gga_delay 是gga延迟时间
ggatime = mpu_Data_value->time;//有效定位时间
reget_sateliti_count++;//重获卫星开始计数
//if (norm(gpsvn) > 10.0&&gga->h_DOP < 5.0&&gga->num_satellite>4)// 航向强制约束
//{kf.sins.att.k = -atan2(gpsvn.i, gpsvn.j);//航向强制约束，这里忽略了航向失准角问题
// kf.sins.qnb = a2qua(kf.sins.att);}
}
}//gps_ok_flag
if ((timu_m - ggatime) > 2)
{
double vnij = norm(gpsvn);
kf.sins.vn.i = vnij * sin(-kf.sins.att.k);
kf.sins.vn.j = vnij * cos(-kf.sins.att.k);
}
if (KfInit)
{
Ts = timu_m - last_timu_m;
if (Ts < (1.0 / Freq_imu)) Ts = 1.0/Freq_imu;
kf.Update(&wm, &vm, 1, Ts);//KF 解算
if (Ts > 2.0) Ts = 1.0;
kf.Update(&wm, &vm, 1, Ts);//KF 解算
last_timu_m =timu_m;
#ifdef WIN32
if ((timu_m - ggatime) < INS_time)// &&reget_sateliti_count > Reget_satelite) //卫星失锁大于10s不出定位，重获卫星3s，不出定位
//if (KfInit && (timu_m - ggatime) < INS_time && (OutCount >= Freq_imu / FreqOut + 1) && reget_sateliti_count > Reget_satelite)
if ((timu_m - ggatime) < INS_time )//&&reget_sateliti_count > Reget_satelite) //卫星失锁大于10s不出定位，重获卫星3s，不出定位
{
CSINS sins_sl = kf.sins;
fins << kf.sins;
fins << kf.sins;//离线处理打印使用
fkf << kf;
fins << kf.sins;
frk << kf.Rt;
//printf("timu_m - ggatime %f\r\n", timu_m - ggatime);
}
#endif
if ((timu_m - ggatime)>1.5)
{
//加速度重新分解成//sl 925 yaw0 = -atan2(gpsvn.i, gpsvn.j) / glv.deg;//deg
double anij = sqrt(kf.sins.an.i*kf.sins.an.i + kf.sins.an.j*kf.sins.an.j);
kf.sins.an.i = anij * sin(-kf.sins.att.k);
kf.sins.an.j = anij * cos(-kf.sins.att.k);
double vnij = sqrt(kf.sins.vn.i*kf.sins.vn.i + kf.sins.vn.j*kf.sins.vn.j);
kf.sins.vn.i = anij * sin(-kf.sins.att.k);
kf.sins.vn.j = anij * cos(-kf.sins.att.k);
}
if ((timu_m - ggatime) > INS_time)
{
reget_sateliti_count = 0;
......@@ -272,10 +274,9 @@ Out_Data* SINS_API(MPU_Data_value *mpu_Data_value, gga_t *gga, vtg_t *vtg, dhv_t
kf.sins.vn.i = gpsvn.i;
kf.sins.vn.j = gpsvn.j;
kf.sins.vn.k = gpsvn.k;
//KfInit = false;
//count1KfInit = true;
}
if (timu_m - ggatime > 120.0)//2min失锁需要停止惯导
if (timu_m - ggatime >600.0)//10min失锁需要停止惯导
{
KfInit = false;
count1KfInit = true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Menu

Help