GNSS和INS松组合代码

C语言的GNSS和INS的松组合代码，C语言的GNSS和INS的松组合代码

#include “Filter.h“

bool LeastSquareFilter（matrix * B matrix * P matrix * w int GPSEPHEMSVCanBeUsednum matrix * x double * Dop double * sigma0）
{
	matrix BT;
	BT.col = B->row; BT.row = B->col; BT.num = B->num;
	BT.element = （double*）malloc（sizeof（double）*BT.num）;
	MatrixTranspose（B &BT）;
	matrix Mul1;
	Mul1.row = BT.row; Mul1.col = P->col;  Mul1.num = Mul1.col*Mul1.row;
	Mul1.element = （double*）malloc（sizeof（double）*Mul1.num）;
	MatrixMultiply（&BT P &Mul1）;
	matrix Mul2;
	Mul2.row = Mul1.row; Mul2.col = B->col; Mul2.num = Mul2.col*Mul2.row;
	Mul2.element = （double*）malloc（sizeof（double）*Mul2.num）;
	MatrixMultiply（&Mul1 B &Mul2）;
	matrix y;
	y.col = Mul2.col; y.row = Mul2.col; y.num = Mul2.col*Mul2.col;
	y.element = （double*）malloc（sizeof（double）*y.num）;

	if （MatrixInv（B->col Mul2.element y.element） == 0）
	{
		free（y.element）; 
		free（Mul1.element）; 
		free（Mul2.element）; 
		free（BT.element）;
		return false;
	}
	matrix yBT;
	yBT.row = y.row; yBT.col = BT.col; yBT.num = yBT.col*yBT.row;
	yBT.element = （double*）malloc（sizeof（double）*yBT.num）;
	MatrixMultiply（&y &BT &yBT）;
	matrix D;
	D.row = yBT.row; D.col = P->col; D.num = D.col*D.row;
	D.element = （double*）malloc（sizeof（double）*D.num）;
	MatrixMultiply（&yBT P &D）;
	MatrixMultiply（&D w x）;

	if （GPSEPHEMSVCanBeUsednum > 4）
	{
		matrix wT;
		wT.col = w->row; wT.row = w->col; wT.num = w->num;
		wT.element = （double*）malloc（sizeof（double）*wT.num）;
		MatrixTranspose（w &wT）;
		matrix VTPV;
		VTPV.col = w->col; VTPV.row = wT.row; VTPV.num = VTPV.col*VTPV.row;
		VTPV.element = （double*）malloc（sizeof（double）*VTPV.num）;
		MatrixMultiply（&wT w &VTPV）;
		*sigma0 = sqrt（VTPV.element[0] / （GPSEPHEMSVCanBeUsednum - 4））;
		*Dop = sqrt（y.element[0] + y.element[5] + y.element[10]）;//y矩阵就是Q矩阵即协因数矩阵;
		free（wT.element）; 
		free（VTPV.element）;
	}
	free（Mul1.element）; 
	free（Mul2.element）; 
	free（BT.element）;
	free（D.element）; 
	free（yBT.element）;
	free（y.element）;
	return true;
}


void KalmanFilter（matrix * phik_k_1 matrix * Hk matrix * Zk matrix * Xk_1 matrix * DXk_1 matrix * Dek_1 matrix * Ddelta_k matrix * Taok_k_1 matrix * Xk matrix * DXk）
{
	matrix Xk_k_1;
	Xk_k_1.col = Xk_1->col; Xk_k_1.row = Xk_1->row; Xk_k_1.num = Xk_1->num;
	Xk_k_1.element = （double*）malloc（sizeof（double）*Xk_k_1.num）;
	MatrixMultiply（phik_k_1 Xk_1 &Xk_k_1）;
	matrix phik_k_1T;
	phik_k_1T.col = phik_k_1->row; phik_k_1T.row = phik_k_1->col; phik_k_1T.num = phik_k_1->num;
	phik_k_1T.element = （double*）malloc（sizeof（double）*phik_k_1T.num）;
	MatrixTranspose（phik_k_1 &phik_k_1T）;
	matrix phi_D_phiT;
	phi_D_phiT.row = phik_k_1->row; phi_D_phiT.col = phik_k_1T.col; phi_D_phiT.num = phi_D_phiT.col*phi_D_phiT.row;
	phi_D_phiT.element = （double*）malloc（sizeof（double）*phi_D_phiT.num）;
	MatrixMultiply3（phik_k_1 DXk_1 &phik_k_1T &phi_D_phiT）;

	matrix TaoT;
	TaoT.row = Taok_k_1->col; TaoT.col = Taok_k_1->row; TaoT.num = TaoT.col*TaoT.row;
	

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

    ..A..H.     78848  2018-01-28 23:13  LooseIntegratedNav加入GPS速度观测值\.vs\LooseIntegratedNav\v14\.suo

     文件       1117  2018-01-28 23:05  LooseIntegratedNav加入GPS速度观测值\AllHeader.h

     文件    6717168  2018-01-05 12:51  LooseIntegratedNav加入GPS速度观测值\CPT1218imu.txt

     文件      19749  2018-01-05 15:28  LooseIntegratedNav加入GPS速度观测值\Debug\Filter.obj

     文件     115191  2018-01-28 23:09  LooseIntegratedNav加入GPS速度观测值\Debug\INS.obj

     文件      95002  2017-11-27 21:18  LooseIntegratedNav加入GPS速度观测值\Debug\ins.obj.enc

     文件      13882  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\CL.command.1.tlog

     文件      74912  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\CL.read.1.tlog

     文件      21002  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\CL.write.1.tlog

     文件       7056  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\link.command.1.tlog

     文件      13886  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\link.read.1.tlog

     文件       3912  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\link.write.1.tlog

     文件        241  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseInt.ED0F9E78.tlog\LooseIntegratedNav.lastbuildstate

     文件       1748  2017-11-25 10:14  LooseIntegratedNav加入GPS速度观测值\Debug\LooseIntegratedNav.Build.CppClean.log

     文件     189952  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseIntegratedNav.exe

     文件     599912  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseIntegratedNav.ilk

     文件        152  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseIntegratedNav.log

     文件    1265664  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\LooseIntegratedNav.pdb

     文件      24096  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\main.obj

     文件      18175  2017-11-27 21:18  LooseIntegratedNav加入GPS速度观测值\Debug\main.obj.enc

     文件      57497  2018-01-28 23:09  LooseIntegratedNav加入GPS速度观测值\Debug\matrix.obj

     文件      53846  2017-11-27 21:18  LooseIntegratedNav加入GPS速度观测值\Debug\matrix.obj.enc

     文件     223837  2018-01-28 23:09  LooseIntegratedNav加入GPS速度观测值\Debug\ReadFile.obj

     文件      17126  2018-01-05 19:37  LooseIntegratedNav加入GPS速度观测值\Debug\readfile.obj.enc

     文件      24602  2018-01-28 23:09  LooseIntegratedNav加入GPS速度观测值\Debug\Reference.obj

     文件      24522  2017-11-27 21:18  LooseIntegratedNav加入GPS速度观测值\Debug\reference.obj.enc

     文件      20993  2018-01-28 23:09  LooseIntegratedNav加入GPS速度观测值\Debug\TimeChange.obj

     文件      20913  2017-11-27 21:18  LooseIntegratedNav加入GPS速度观测值\Debug\timechange.obj.enc

     文件     437248  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\vc140.idb

     文件     438272  2018-01-28 23:12  LooseIntegratedNav加入GPS速度观测值\Debug\vc140.pdb

............此处省略33个文件信息