#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矩陣即協(xié)因數(shù)矩陣;
		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個文件信息