📄 gpsfuncs.cpp
字号:
/***********************************************************************
GPS RECEIVER (GPSRCVR) Ver. 1.18
See comments in GPSRCVR.CPP
12 Channel All-in-View GPS Receiver Program based on Mitel GP2021
chipset
Clifford Kelley <cwkelley@earthlink.net>
This program is licensed under BSD LICENSE
This LICENSE file must be included with the GPSRCVR code.
**********************************************************************/
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <conio.h>
#include <math.h>
#include <string.h>
#include <io.h>
#include <Dos.h>
#ifndef DJGPP
#include "CMatrix3.h"
#endif
#include "GP2021.h"
#include "consts.h"
#include "structs.h"
#include "globals.h"
#include "gpsfuncs.h"
//
// binary constants for nav message decoding
//
double const c_2p12 = 4096;
double const c_2p4 = 16;
double const c_2m5 = 0.03125;
double const c_2m11 = 4.8828125e-4;
double const c_2m19 = 1.9073486328125e-6;
double const c_2m20 = 9.5367431640625e-7;
double const c_2m21 = 4.76837158203125e-7;
double const c_2m23 = 1.19209289550781e-7;
double const c_2m24 = 5.96046447753906e-8;
double const c_2m27 = 7.45058059692383e-9;
double const c_2m29 = 1.86264514923096e-9;
double const c_2m30 = 9.31322574615479e-10;
double const c_2m31 = 4.65661287307739e-10;
double const c_2m33 = 1.16415321826935E-10;
double const c_2m38 = 3.63797880709171e-12;
double const c_2m43 = 1.13686837721616e-13;
double const c_2m50 = 8.881784197e-16;
double const c_2m55 = 2.77555756156289e-17;
double const a=6378137.0,b=6356752.314; // WGS-84 ellipsoid parameters
#ifdef VCPP
Matrix Pk(8,8),Pk1(8,8),Phi(8,8),Qk(8,8),I(8,8); // covariance, partial, transition, Process noise, identity matrices
ColumnVector Xk(8),Xk1(8); // state vector
pvt Kalman_filter(int n_meas)
{
int i,j,N_Pr,N_dPr,n;
double alpha,r;
double Sxx,Syy,Szz,Vxx,Vyy,Vzz;
double Sg,Sgs,Ss;
double Pr_residual[13],dPr_residual[13];
pvt result;
// pvt least_sq;
// llh position_L;
// ecef position_E;
/*
if (n_meas>3)
{
least_sq=pos_vel_time_a(n_meas);
position_E.x=least_sq.x;
position_E.y=least_sq.y;
position_E.z=least_sq.z;
position_L=ecef_to_llh(position_E);
fprintf(kalm,"LSQ Position %lf, %lf, %lf freq delta=%lf\n",position_L.lat*r_to_d,position_L.lon*r_to_d,position_L.hae,least_sq.df);
}
*/
if (init==1)
{
Sxx = Syy = Szz = 0.01*TIC_dt*TIC_dt;
Vxx = Vyy = Vzz = 0.01*TIC_dt*TIC_dt;
Sg = 0.0001*TIC_dt*TIC_dt;
Sgs = 0.0;
Ss = 0.01*TIC_dt*TIC_dt;
//
// Process noise, for now keep it fixed
//
fprintf(kalm," In Kalman filter init\n");
Qk(1,1) =Sxx ;Qk(1,2)=0.0 ;Qk(1,3)=0.0 ;Qk(1,4)=0.0 ;Qk(1,5)=0.0 ;Qk(1,6)=0.0 ;Qk(1,7)=0.0 ;Qk(1,8)=0.0;
Qk(2,1) =0.0 ;Qk(2,2)=Syy ;Qk(2,3)=0.0 ;Qk(2,4)=0.0 ;Qk(2,5)=0.0 ;Qk(2,6)=0.0 ;Qk(2,7)=0.0 ;Qk(2,8)=0.0;
Qk(3,1) =0.0 ;Qk(3,2)=0.0 ;Qk(3,3)=Szz ;Qk(3,4)=0.0 ;Qk(3,5)=0.0 ;Qk(3,6)=0.0 ;Qk(3,7)=0.0 ;Qk(3,8)=0.0;
Qk(4,1) =0.0 ;Qk(4,2)=0.0 ;Qk(4,3)=0.0 ;Qk(4,4)=Vxx ;Qk(4,5)=0.0 ;Qk(4,6)=0.0 ;Qk(4,7)=0.0 ;Qk(4,8)=0.0;
Qk(5,1) =0.0 ;Qk(5,2)=0.0 ;Qk(5,3)=0.0 ;Qk(5,4)=0.0 ;Qk(5,5)=Vyy ;Qk(5,6)=0.0 ;Qk(5,7)=0.0 ;Qk(5,8)=0.0;
Qk(6,1) =0.0 ;Qk(6,2)=0.0 ;Qk(6,3)=0.0 ;Qk(6,4)=0.0 ;Qk(6,5)=0.0 ;Qk(6,6)=Vzz ;Qk(6,7)=0.0 ;Qk(6,8)=0.0;
Qk(7,1) =0.0 ;Qk(7,2)=0.0 ;Qk(7,3)=0.0 ;Qk(7,4)=0.0 ;Qk(7,5)=0.0 ;Qk(7,6)=0.0 ;Qk(7,7)=Sg ;Qk(7,8)=0.0;
Qk(8,1) =0.0 ;Qk(8,2)=0.0 ;Qk(8,3)=0.0 ;Qk(8,4)=0.0 ;Qk(8,5)=0.0 ;Qk(8,6)=0.0 ;Qk(8,7)=0.0 ;Qk(8,8)=Ss;
if (n_meas>3)
{
result=pos_vel_time_a(n_meas);
// fprintf(kalm," In Kalman filter init after pvt\n");
// fprintf(kalm," m_time[1]=%lf dclock=%lf dfclock=%lf\n",m_time[1],result.dt,result.df);
init=0;
Xk1(1)=result.x;
Xk1(2)=result.y;
Xk1(3)=result.z;
Xk1(4)=result.xv;
Xk1(5)=result.yv;
Xk1(6)=result.zv;
Xk1(7)=result.dt;
Xk1(8)=result.df;
}
else
{
Xk(1)=rec_pos_xyz.x;
Xk(2)=rec_pos_xyz.y;
Xk(3)=rec_pos_xyz.z;
Xk(4)=0.0-Xk(2)*omegae;
Xk(5)=0.0+Xk(1)*omegae;
Xk(6)=0.0;
Xk(7)=0.0;
// Xk(8)=0.0;
}
// Initialize transition matrix for TIC_dt second updates;
for (i=1;i<9;i++)
{
for (j=1;j<9;j++)
{
if (i==j)
{
Phi(i,j)=1.0;
I(i,j)=1.0;
}
else
{
Phi(i,j)=0.0;
I(i,j)=0.0;
}
}
}
Phi(1,2)=+omegae*TIC_dt;Phi(2,1)=-omegae*TIC_dt;
Phi(1,4)=Phi(2,5)=Phi(3,6)=TIC_dt;
Phi(7,8)=TIC_dt;
Xk = Phi*Xk1; // project ahead
// Initialize Covariance matrix
for (i=1;i<9;i++)
{
for (j=1;j<9;j++)
{
if (i==j)
{
if (i<4) Pk(i,j)=10000.0; // assume 100 m position sigma
else if (i<7) Pk(i,j)=100.0; // assume 10 m/s velocity sigma
else Pk(i,j)=10.0;
}
else Pk(i,j)=0.0;
}
}
}
else // Go into Kalman Filter
{
// compute Pr measurements
// fprintf(kalm," %d Pr measurements, clock offset = %lf\n",n_meas,Xk(7));
N_Pr=0;
for (i=1;i<=n_meas;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=-(Xk(7)/c+dt[i])*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-Xk(1),2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-Xk(2),2)+
pow(track_sat[i].z-Xk(3),2));
Pr_residual[i]=r-dt[i]*c-Xk(7);
// fprintf(kalm,"%lf %d Pr residual=%lf, dt = %lf\n",m_time[1],i,Pr_residual[i],dt[i]);
if (fabs(Pr_residual[i])<250.0)
{
N_Pr++;
}
}
// compute delta Pr measurements
// fprintf(kalm," %d dPr measurements, clock freq offset = %lf\n",n_meas,Xk(8));
N_dPr=0;
for (i=1;i<=n_meas;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=-(Xk(7)/c+dt[i])*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-Xk(1),2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-Xk(2),2)+
pow(track_sat[i].z-Xk(3),2));
dPr_residual[i]=((track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-Xk(1))*(d_sat[i].x-Xk(4))+
(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-Xk(2))*(d_sat[i].y-Xk(5))+
(track_sat[i].z-Xk(3))*(d_sat[i].z-Xk(6)))/r-meas_dop[i]*lambda-Xk(8);
// fprintf(kalm," %i residual=%lf\n",i,dPr_residual[i]);
if (fabs(dPr_residual[i])<10.0)
{
N_dPr++;
}
}
//
// fprintf(kalm," N=%d M=%d\n",N,M);
if (N_Pr==0 && n_meas>3)
{
result=pos_vel_time_a(n_meas);
// init=1;
Xk1(1)=result.x;
Xk1(2)=result.y;
Xk1(3)=result.z;
Xk1(4)=result.xv;
Xk1(5)=result.yv;
Xk1(6)=result.zv;
Xk1(7)=result.dt;
Xk1(8)=result.df;
Xk = Phi*Xk1; // project ahead
for (i=1;i<9;i++)
{
for (j=1;j<9;j++)
{
if (i==j)
{
if (i<4) Pk(i,j)=10000.0; // assume 100 m position sigma
else if (i<7) Pk(i,j)=100.0; // assume 10 m/s velocity sigma
else Pk(i,j)=10.0;
}
else Pk(i,j)=0.0;
}
}
}
else if (N_Pr+N_dPr>0)
{
Matrix Kk(N_Pr+N_dPr,N_Pr+N_dPr),Rk(N_Pr+N_dPr,N_Pr+N_dPr),Hk(N_Pr+N_dPr,8);
ColumnVector Zk(N_Pr+N_dPr);
// Initialize Rk, non zeros will be filled in later
for (i=1;i<=N_Pr+N_dPr;i++)
{
for (j=1;j<=N_Pr+N_dPr;j++)
{
Rk(i,j)=0.0;
}
}
n=1;
for (i=1;i<=n_meas;i++)
{
//
// Set up Pr measurements
//
if (fabs(Pr_residual[i])<250.0)
{
Zk(n)=Pr_residual[i];
Hk(n,1)=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-Xk(1))/r;
Hk(n,2)=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-Xk(2))/r;
Hk(n,3)=(track_sat[i].z-Xk(3))/r;
Hk(n,7)=1.0;
Rk(n,n)=9.0;
n++;
}
}
// compute delta Pr measurements
for (i=1;i<=n_meas;i++)
{
//
// Set up dPr measurements
//
if (fabs(dPr_residual[i])<10.0)
{
Zk(n)=dPr_residual[i];
Hk(n,4)=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-Xk(1))/r;
Hk(n,5)=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-Xk(2))/r;
Hk(n,6)=(track_sat[i].z-Xk(3))/r;
Hk(n,8)=1.0;
Rk(n,n)=0.01; // for now assume 0.1 m dPr measurement sigma
n++;
}
}
Kk = Pk*Hk.t()*(Hk*Pk*Hk.t()+Rk).i();
Xk1 = Xk+Kk*Zk; // take into account the measurement
Pk1 = (I-Kk*Hk)*Pk;
}
else
{
Xk1=Xk;
}
// project ahead
Xk = Phi*Xk1;
Pk = Phi*Pk1*Phi.t()+Qk;
}
result.x=Xk(1);
result.y=Xk(2);
result.z=Xk(3);
result.xv=Xk(4)+Xk(2)*omegae;
result.yv=Xk(5)-Xk(1)*omegae;
result.zv=Xk(6);
result.dt=Xk(7);
result.df=Xk(8);
return(result);
}
/*******************************************************************************
FUNCTION pos_vel_time_a(int nsl)
RETURNS None.
PARAMETERS
nsl int
PURPOSE
This routine processes the all-in-view pseudorange to arrive
at a receiver position
INPUTS:
pseudo_range[nsl] Vector of measured range from satellites to the receiver
sat_location[nsl][3] Array of satellite locations in ECEF when the signal
was sent
nsl number of satellites used
OUTPUTS:
RP[3] VECTOR OF RECEIVER POSITION IN ECEF (X,Y,Z)
CBIAS RECEIVER CLOCK BIAS FROM GPS TIME
VARIABLES USED:
C SPEED OF LIGHT IN VACUUM IN M/S
S[6][5] MATRIX USED FOR SOLVING FOR RECEIVER POSITION CORRECTION
B[5] RESULTING RECEIVER CLOCK BIAS & POSITION CORRECTIONS
X,Y,Z TEMPORARY RECEIVER POSITION
T TEMPORARY RECEIVER CLOCK BIAS
R[5] RANGE FROM RECEIVER TO SATELLITES
IF THE POSITION CANNOT BE DETERMINED THE RESULT OF RP
WILL BE (0,0,0) THE CENTER OF THE EARTH
WRITTEN BY
Clifford Kelley
*******************************************************************************/
pvt pos_vel_time_a( int nsl)
{
double r,correct_mag,alpha;
int i,j,nits;
pvt result;
Matrix H(nsl,4),G(4,4);
ColumnVector e(nsl),P(4),dP(4),V(4);
//
// USE ITERATIVE APPROACH TO SOLVING FOR THE POSITION OF
// THE RECEIVER
//
nits=0;
P(1)=rec_pos_xyz.x;
P(2)=rec_pos_xyz.y;
P(3)=rec_pos_xyz.z;
P(4)=cbias;
do
{
for (i=1;i<=nsl;i++)
{
//
// Compute range in ECI at the time of arrival at the receiver
//
alpha=-(P(4)/c+dt[i])*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-P(1),2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-P(2),2)+
pow(track_sat[i].z-P(3),2));
e(i)=r-dt[i]*c-P(4);
H(i,1)=(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-P(1))/r;
H(i,2)=(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-P(2))/r;
H(i,3)=(track_sat[i].z-P(3))/r;
H(i,4)=1.0;
}
G = (H.t()*H).i()*H.t();
dP = G*e;
P = P + dP;
correct_mag=sqrt(dP(1)*dP(1)+dP(2)*dP(2)+dP(3)*dP(3)+dP(4)*dP(4));
} while ( correct_mag > 0.01 && correct_mag < 1.e9 && nits < 10);
result.x=P(1);
result.y=P(2);
result.z=P(3);
result.dt=P(4);
//
// Now for Velocity
//
for (i=1;i<=nsl;i++)
{
alpha=-(P(4)/c+dt[i])*omegae;
r=sqrt(pow(track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-P(1),2)+
pow(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-P(2),2)+
pow(track_sat[i].z-P(3),2));
e(i)=((track_sat[i].x*cos(alpha)-track_sat[i].y*sin(alpha)-P(1))*d_sat[i].x+
(track_sat[i].y*cos(alpha)+track_sat[i].x*sin(alpha)-P(2))*d_sat[i].y+
(track_sat[i].z-P(3))*d_sat[i].z)/r-meas_dop[i]*lambda;
}
V = G*e;
result.xv=V(1);//+P(2)*omegae; // get rid of earth
result.yv=V(2);//-P(1)*omegae; // rotation rate
result.zv=V(3);
result.df=V(4); // frequency error
return(result);
}
#endif
/*******************************************************************************
FUNCTION satfind()
RETURNS None.
PARAMETERS None.
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -