psins导航算法源码C语言、matlab

#include “KFApp.h“

/***************************  class CCarAHRS  *********************************/
CCarAHRS::CCarAHRS（double ts）:CTDKF（1613）
{
	levelAlignOK = yawAlignOK = initPosOK = FALSE;
	iter = -2;  ifn = 0;
	measGPSVn = measGPSPos = measINSvn = measCMvb = measMag = O31;
	double sts = sqrt（ts）;
	Pmax.Set2（10.0*glv.deg10.0*glv.deg30.0*glv.deg 50.050.050.0 1.0e4/glv.Re1.0e4/glv.Re1.0e4 
		1000.0*glv.dph1000.0*glv.dph1000.0*glv.dph 100.0*glv.mg100.0*glv.mg100.0*glv.mg 10000.0*glv.ppm）;
	Pmin.Set2（1.0*glv.min1.0*glv.min10.0*glv.min 0.010.010.1 1.0/glv.Re1.0/glv.Re0.1 
		0.1*glv.dph0.1*glv.dph0.1*glv.dph 0.1*glv.mg0.1*glv.mg0.1*glv.mg 10*glv.ppm）;
	Pk.SetDiag2（10.0*glv.deg10.0*glv.deg10.0*glv.deg 1.01.01.0 100.0/glv.Re100.0/glv.Re100.0 
		100.0*glv.dph100.0*glv.dph100.0*glv.dph 10.0*glv.mg10.0*glv.mg10.0*glv.mg 1000*glv.ppm）;
	Qt.Set2（1.1*glv.dpsh1.1*glv.dpsh1.1*glv.dpsh 500.0*glv.ugpsHz500.0*glv.ugpsHz500.0*glv.ugpsHz 0.00.00.0
		0.00.00.0 100.0*glv.ugpsh100.0*glv.ugpsh100.0*glv.ugpsh 100.0*glv.ppmpsh）;
	Rk.Set2（0.5 0.5 0.5 10.0/glv.Re 10.0/glv.Re 10.0 0.1/sts 0.1/sts 0.1/sts 10.0/sts 1000.0/sts 1.0/sts 1.0*glv.deg）;
	SetHk（）; 
	Hk（ZUPT3） = Hk（ZUPT+14） = Hk（ZUPT+25） = 1.0;
	Hk（GPSYAW2） = -1.0;  measGPSYaw = 0;
}

void CCarAHRS::SetMeasGPSVn（CVect3& vnGPS）
{
	measGPSVn = vnGPS;
	tmeas.dd[GPSVN] = tk;
}

void CCarAHRS::SetMeasGPSPos（CVect3& posGPS）
{
	measGPSPos = posGPS;
	tmeas.dd[GPSPOS] = tk;
	if（!initPosOK）
	{
		sins.pos = posGPS;  initPosOK = TRUE;
	}
}

void CCarAHRS::SetMeasZUPT（void）
{
	measINSvn = sins.vn;
	tmeas.dd[ZUPT] = tk;
}

void CCarAHRS::SetMeasMag（CVect3& mag）
{
	measMag = mag;
}

void CCarAHRS::SetMeasMC（void）
{
	Hk（CARMC+03） = sins.Cnb.e00; Hk（CARMC+04） = sins.Cnb.e10; Hk（CARMC+05） = sins.Cnb.e20;
	Hk（CARMC+13） = sins.Cnb.e01; Hk（CARMC+14） = sins.Cnb.e11; Hk（CARMC+15） = sins.Cnb.e21;
	Hk（CARMC+23） = sins.Cnb.e02; Hk（CARMC+24） = sins.Cnb.e12; Hk（CARMC+25） = sins.Cnb.e22;
	measCMvb = sins.vb;
	if（measCMvb.j>20） measCMvb.j-=20;
	else if（measCMvb.j<-1） measCMvb.j-=-1;
	tmeas.dd[CARMC] = tk;
}

void CCarAHRS::SetMeasGPSYaw（double gpsYaw）
{
	measGPSYaw = gpsYaw;
	tmeas.dd[GPSYAW] = tk;
}

void CCarAHRS::MeasRearrange（CSINS &sins）
{
	if（yawAlignOK && measGPSVn.k!=0 && tk-tmeas.dd[GPSVN]<0.5）   // GPSVn
	{
		*（CVect3*）&Zk.dd[GPSVN] = sins.vn-measGPSVn;
		measGPSVn = O31; SetMeasFlag（0x07）;
	}
	if（yawAlignOK && measGPSPos.k!=0 && tk-tmeas.dd[GPSPOS]<0.5）   // GPSPos
	{
		*（CVect3*）&Zk.dd[GPSPOS] = sins.pos-measGPSPos-sins.eth.vn2dpos（sins.vntk-tmeas.dd[GPSPOS]-sins.nts）;
		measGPSPos = O31; SetMeasFlag（0x38）;
	}
	if（levelAlignOK && measINSvn.k!=0 && tk-tmeas.dd[ZUPT]<0.1）   // ZUPT
	{
		*（CVect3*）&Zk.dd[ZUPT] = measINSvn-sins.an*（tk-tmeas.dd[ZUPT]-sins.nts）;
		measINSvn = O31; SetMeasFlag（0x01C0）;
	}
	else if（levelAlignOK && measCMvb.k!=0 && tk-tmeas.dd[CARMC]<0.1 &&

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-09-29 11:22  psins源码
     目录           0  2018-09-29 11:22  psins源码PSINS-CPP
     目录           0  2018-09-29 11:22  psins源码PSINS-CPPPSINS-CPP
     目录           0  2018-09-29 11:22  psins源码PSINS-CPPPSINS-CPPCoreSource
     文件        4310  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPCoreSourceKFApp.cpp
     文件         912  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPCoreSourceKFApp.h
     文件        2195  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPCoreSourceMain.cpp
     文件       40431  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPCoreSourcePSINS.cpp
     文件       12299  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPCoreSourcePSINS.h
     文件    20733089  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPdata1.txt
     目录           0  2018-09-29 11:22  psins源码PSINS-CPPPSINS-CPPKeilV453
     目录           0  2018-09-29 11:22  psins源码PSINS-CPPPSINS-CPPKeilV453out
     文件       50827  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outkfapp.crf
     文件         387  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outkfapp.d
     文件       47892  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outkfapp.o
     文件       50097  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outmain.crf
     文件         377  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outmain.d
     文件       38840  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outmain.o
     文件       84979  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.crf
     文件         349  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.d
     文件      248944  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.htm
     文件         324  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.lnp
     文件      182060  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.o
     文件       16874  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.plg
     文件        1094  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outpsins.tra
     文件          28  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outStartup.d
     文件        3200  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453outStartup.o
     文件      125113  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453psins.map
     文件        5953  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453psins.plg
     文件       70680  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453psins.uvgui.Cui
     文件       70681  2018-02-05 08:41  psins源码PSINS-CPPPSINS-CPPKeilV453psins.uvgui_Cui.bak
............此处省略121个文件信息