GPS、BDS组合伪距单点定位

#include
#include
#include
#include
#include
#include
#include“matrix.h“
using namespace std;
#include“mystruct.h“
//以下矩阵要用------------------
using namespace std;           //
#ifndef _NO_NAMESPACE          //
using namespace math;          //
#define STD std                //
#else                          //
#define STD                    //
#endif                         //
                               //
#ifndef _NO_TEMPLATE           //
typedef matrix Matrix; // 
#else                          //
typedef matrix Matrix;         // 
#endif//--------
///////////////////////
/////////////////////////////////////////////////
void GEO（int& posktime_gps& t2double& trsta_polar& spvector& vn）
{
	//double pi=3.1415926535898;
	double tk=t2.num_sec-vn[posk].TOE.num_sec+（t2.num_week-vn[posk].TOE.num_week）*604800;//正常减出来是-14.050
	//double tk=-14.5;
	double const GM=3.9860047e14;
	double const  we=7.29211514767e-5;//地球自转角速度
	double const pi=3.1415926535898;
	double const c=2.99792458e8;//光速
	double n0=sqrt（GM）/pow（vn[posk].sqrtA3）;
	double e_n=n0+vn[posk].delta_n;
	if（tk>302400） tk=tk-604800;
	if（tk<-302400） tk=tk+604800;
	double M=vn[posk].M0+tk*e_n;//平近点角
	//cout<<“M平近点角=“<	double EE0Ek;
	/////迭代偏近点角
	E0=M;
	 do
	 {Ek=E0;
	 E=M+vn[posk].e*sin（Ek）;
	 E0=E;
	 }while（fabs（E-Ek）>1e-12）;//fabs浮点数取绝对值
	// cout<<“偏近点角E“<	 double vk=atan2（（sqrt（1-vn[posk].e*vn[posk].e）*sin（E））cos（E）-vn[posk].e）;//注意atan和atan2的区别
	 //cout<<“真近点角“<	 double faik=vk+vn[posk].w;
	 //cout<<“升交距角“<	 /////摄动改正项u，r，i计算
	 double duk=vn[posk].Cuc*cos（2*faik）+vn[posk].Cus*sin（2*faik）;//升交距角u的摄动量
	 double drk=vn[posk].Crc*cos（2*faik）+vn[posk].Crs*sin（2*faik）;//卫星矢径r的摄动量
	 double dik=vn[posk].Cic*cos（2*faik）+vn[posk].Cis*sin（2*faik）;//轨道倾角i的摄动量
	 //经过摄动改正后的
	 double uk=duk+faik;
	 double rk=vn[posk].sqrtA*vn[posk].sqrtA*（1-vn[posk].e*cos（E））+drk;
	 double ik=vn[posk].i0+dik+vn[posk].i_DOT*tk;
	// cout<<“升交距角uk“<	 //卫星在轨道平面坐标系的坐标
	 double xkd=rk*cos（uk）;
	 double ykd=rk*sin（uk）;
	 ////改正后升交点经度
	// tk=-14.50000011920929;
	 double tempp1=vn[posk].OMEGA;
	 double tempp2=vn[posk].OMEGA_DOT;
	double tempp3= t2.num_sec;

	 double omgak=vn[posk].OMEGA+（vn[posk].OMEGA_DOT）*tk-we*vn[posk].TOE.num_sec;
	  //omgak=-34.497361011748382;
	  //计算卫星在地心固定坐标系中的直角坐标
	 double xk=xkd*cos（omgak）-ykd*cos（ik）*sin（omgak）;
	 double yk=xkd*sin（omgak）+ykd*cos（ik）*cos（omgak）;
	 double zk=ykd*sin（ik）;
	 
	 ///////计算卫星在CGCS2000中的坐标
	 //double Rx[3][3] = {{100}{0cos（-5.0 / 180 * pi）sin（-5.0 / 180 * pi）}{0-sin（-5.0 / 180 * pi）cos（-5.0 / 180 * pi）}}; //C++中cossinasinacos这些三角函数操作的是弧度而非角度！！！
		//double Rz[3][3] = {{cos（we * tk）sin（we * tk）0}{-sin（we * tk）cos（we * tk）0}{001}};
		//double R[3][3]sat[3][1];
		//double satuser[3][1] = {xkykzk};
        Matrix A（33

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2016-08-14 18:14  BD
     目录           0  2016-08-11 19:07  BDBD
     文件        4362  2016-08-11 19:08  BDBDBD.vcxproj
     文件        1749  2016-08-11 19:08  BDBDBD.vcxproj.filters
     目录           0  2016-08-14 17:20  BDBDDebug
     文件        1470  2016-08-11 19:11  BDBDDebugBD.Build.CppClean.log
     文件          65  2016-08-14 17:20  BDBDDebugBD.lastbuildstate
     文件        7414  2016-08-14 17:20  BDBDDebugBD.log
     文件       30568  2016-08-14 17:20  BDBDDebugCL.read.1.tlog
     文件        2402  2016-08-14 17:20  BDBDDebugCL.write.1.tlog
     文件      181144  2016-08-11 19:11  BDBDDebugGEO.obj
     文件      148367  2016-08-11 20:32  BDBDDebugGPS_sat_pos.obj
     文件        3286  2016-08-14 17:20  BDBDDebugcl.command.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink-cvtres.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink-cvtres.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink-rc.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink-rc.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772-cvtres.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772-cvtres.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772-rc.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772-rc.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.3772.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384-cvtres.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384-cvtres.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384-rc.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384-rc.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.4384.write.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.6616-cvtres.read.1.tlog
     文件           2  2016-08-14 17:20  BDBDDebuglink.6616-cvtres.write.1.tlog
............此处省略35个文件信息