/*矩阵减运算*/
public static matrix operator -(matrix a, matrix b)
{
int row = a.rows, column = a.columns;
if (row == b.rows && column == b.columns)
{
matrix c = new matrix(row, column);
for (int i = 1; i <= row; i++)
{
for (int j = 1; j <= column; j++)
{
c[i, j] = a[i, j] - b[i, j];
}
}
return(c);
}
else
{
return(null);
}
}
public void error_enu(List <result> res)
{
matrix xyz = new matrix(3, 1);
matrix r = new matrix(3, 1);
matrix enu = new matrix(3, 1);
if (realX != null && realY != null && realZ != null)
{
xyz[1, 1] = double.Parse(realX.Text.Trim());
xyz[2, 1] = double.Parse(realY.Text.Trim());
xyz[3, 1] = double.Parse(realZ.Text.Trim());
foreach (var v in res)
{
r[1, 1] = v.X - xyz[1, 1];
r[2, 1] = v.Y - xyz[2, 1];
r[3, 1] = v.Z - xyz[3, 1];
transcoor.xyz2enu(xyz, r, enu);
v.error_E = enu[2, 1];
v.error_N = enu[1, 1];
v.error_U = enu[3, 1];
}
}
}
public static int res_ppp(List <obs_s> obs, station sta, ppp_t p3, int n, double[][] rs, double[][] dts, double[][] azel, double[] vare, erp_t erp,
dcb_t dcb, double[] x, double[] R, double[] v, matrix H, int[] svh)
{
double r, dtrp, vart = Math.Pow(0.01, 2), elmin = 15 * D2R;
double[] rr = new double[3], disp = new double[3], pos = new double[3], e = new double[3], meas = new double[2], varm = new double[2];
double[] dtdx = new double[3], dantr = new double[3], var = new double[nx * 2], dants = new double[2], phw = new double[1];
matrix pos_ = new matrix(3, 1);
int i, j, nv = 0, k, sat;
for (i = 0; i < 3; i++)
{
rr[i] = x[i];
}
/* earth tides correction */ //地球潮汐改正 固体潮
tidedisp(obs[0].t, rtklibcmn.gpst2utc(obs[0].rtkt), rr, erp, disp);
for (i = 0; i < 3; i++)
{
rr[i] += disp[i];
}
transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
for (i = 0; i < 3; i++)
{
pos[i] = pos_[i + 1, 1];
}
for (i = 0; i < 32; i++)
{
p3.vsat[i] = 0;
}
for (i = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
//
if (p3.spp.vsat[sat - 1] == 0 || svh[i] < 0)
{
continue;
}
if ((r = geodist(rs[i], rr, e)) <= 0 || pppcmn.satel(rr, rs[i], azel[i]) < elmin)
{
continue;
}
dtrp = pppcmn.prectrop(obs[i].t, pos, azel[i], x[4], dtdx); //精密对流层模型
pppcmn.satanxpcv(rs[i], rr, pcv, dants, obs[i].sprn); //卫星天线相位偏差，返回每个频率的改正值
pppcmn.antxmodel(pcv, sta.atxdel, azel[i], dantr, sta.anxtype);
pppcmn.windup(obs[i].t, p3.soltime, rs[i], rr, phw);
if (corrmens(obs[i], dcb, pos, dantr, dants, phw, meas, varm) != 1)
{
continue;
}
/* satellite clock and tropospheric delay */ //卫星钟差和电离层延迟
r += -CLIGHT * dts[i][0] + dtrp;
for (j = 0; j < 2; j++)
{
if (meas[j] == 0)
{
continue;
}
v[nv] = meas[j] - r;
for (k = 0; k < nx; k++)
{
H[nv + 1, k + 1] = 0.0;
}
for (k = 0; k < 3; k++)
{
H[nv + 1, k + 1] = -e[k];
}
v[nv] -= x[3]; H[nv + 1, 4] = 1.0;
H[nv + 1, 5] = dtdx[0];
if (j == 0)
{
v[nv] -= x[4 + sat];
H[nv + 1, 5 + sat] = 1.0;
}
var[nv] = varm[j] + vare[i] + vart + varerr(azel[i][1], j);
if (Math.Abs(v[nv]) > 30)
{
continue;
}
if (j == 0)
{
p3.vsat[sat - 1] = 1;
}
nv++;
}
}
for (i = 0; i < nv; i++)
{
R[i] = var[i];
}
return(nv);
}
//obs均已按照PRN号大小排列好
public static int estpos(spp_t spp, List <obs_s> obs, dcb_t dcb, nav_t nav, int n, double[][] rs, double[][] dts,
double[] vare, double[][] azel, int[] vsat, int[] svh)
{
double[] v_ = new double[n], x = new double[4], var = new double[n];
matrix H_ = new matrix(n, 4), H, v, dx, Q, P;
int nv;
double sig;
int i, j, k, stat = 1;
for (i = 0; i < 3; i++)
{
x[i] = spp.rr[i];
}
for (i = 0; i < 10; i++)//迭代
{
nv = rescod(i, obs, nav, n, rs, dts, vare, azel, vsat, dcb, x, H_, v_, var, svh);
if (nv < 4)
{
break; //nv是参与解算的实际卫星数
}
H = new matrix(nv, 4); v = new matrix(nv, 1); P = new matrix(nv, nv);
for (j = 0; j < nv; j++)
{
sig = Math.Sqrt(var[j]);
v[j + 1, 1] = v_[j] / sig;
for (k = 1; k <= 4; k++)
{
H[j + 1, k] = H_[j + 1, k] / sig;
}
}
dx = matrix.inv(matrix.transp(H) * H) * matrix.transp(H) * v;
Q = matrix.transp(H) * H;
for (j = 0; j < 4; j++)
{
x[j] += dx[j + 1, 1];
}
if (Math.Sqrt(dx[1, 1] * dx[1, 1] + dx[2, 1] * dx[2, 1] + dx[3, 1] * dx[3, 1] + dx[4, 1] * dx[4, 1]) < 1E-4)
{
spp.dtr = x[3] / CLIGHT;
for (j = 0; j < 3; j++)
{
spp.rr[j] = x[j];
}
spp.tcurr.gpsec = obs[0].t.gpsec - spp.dtr;
spp.tcur = rtklibcmn.timeadd(obs[0].rtkt, -x[3] / CLIGHT);
for (j = 0; j < 6; j++)
{
spp.tcurr.calend[j] = obs[0].t.calend[j];
}
stat = valsol(azel, vsat, 15.0 * D2R, n, v, nv, 4);
return(stat);
}
}
return(1);
}
public static void upbias_ppp(ppp_t p3, List <obs_s> obs, dcb_t dcb)
{
double[] meas = new double[2], var = new double[2], bias = new double[32], pos = new double[3], rr = new double[3], phw = new double[1];
double offset = 0.0;
matrix pos_ = new matrix(3, 1);
int i, j, k, sat, n = obs.Count;
for (i = 0; i < 32; i++)
{
p3.cyslip[i] = 0;
}
detslp_gf(p3, obs, n);
/* reset phase-bias if expire obs outage counter */
for (i = 0; i < 32; i++)
{
if (++p3.outc[i] > 5)
{
initx(p3, 0.0, 0.0, i + 5);
}
}
for (i = 0; i < 3; i++)
{
rr[i] = p3.spp.rr[i];
}
transcoor.ecef2pos(matrix.Array2matrix(rr), pos_);
for (i = 0; i < 3; i++)
{
pos[i] = pos_[i + 1, 1];
}
for (i = k = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
j = sat + 4;
if ((corrmens(obs[i], dcb, pos, null, null, phw, meas, var)) != 1)
{
continue;
}
;
bias[i] = meas[0] - meas[1];
if (p3.x[j] == 0 || p3.cyslip[sat - 1] == 1)
{
continue;
}
offset += bias[i] - p3.x[j];
k++;
}
/* correct phase-code jump to enssure phase-code coherency */
if (k >= 2 && Math.Abs(offset / k) > 0.0005 * CLIGHT)
{
for (i = 0; i < 32; i++)
{
j = i + 5;
if (p3.x[j] != 0)
{
p3.x[j] += offset / k;
}
}
}
for (i = 0; i < n; i++)
{
sat = int.Parse(obs[i].sprn.Substring(1, 2));
j = sat + 4;
p3.P[j + 1, j + 1] += Math.Pow(1E-4, 2) * Math.Abs(p3.tt);
if (p3.x[j] != 0 && p3.cyslip[sat - 1] != 1)
{
continue; //已初始化且不发生周跳
}
if (bias[i] == 0)
{
continue;
}
initx(p3, bias[i], VAR_BIAS, j);
}
}
public static void filter(matrix x, matrix P, matrix H, matrix R, matrix v, matrix xp, matrix Pp)
{
matrix Q = H * P * matrix.transp(H) + R;
matrix K = P * matrix.transp(H) * matrix.inv(Q);
matrix xp_ = new matrix(x.rows, x.columns);
xp_ = x + K * v;
matrix Pp_ = new matrix(P.rows, P.columns);
Pp_ = P - K * H * P;
for (int i = 1; i <= xp_.rows; i++)
{
for (int j = 1; j <= xp_.columns; j++)
{
xp[i, j] = xp_[i, j];
}
}
for (int i = 1; i <= x.rows; i++)
{
for (int j = 1; j <= x.rows; j++)
{
Pp[i, j] = Pp_[i, j];
}
}
}
/*扩展卡尔曼滤波*/
public static void kalman(double[] x, double[] R, double[] v, matrix P, matrix H, int nv)
{
matrix x_, xp_, P_, Pp_, H_, v_, R_;
int i, j, k;
int[] ix = new int[nx];
for (i = k = 0; i < nx; i++)
{
if (x[i] != 0 && P[i + 1, i + 1] > 0.0)
{
ix[k++] = i;
}
}
x_ = new matrix(k, 1); xp_ = new matrix(k, 1); P_ = new matrix(k, k); Pp_ = new matrix(k, k); H_ = new matrix(nv, k);
v_ = new matrix(nv, 1); R_ = new matrix(nv, nv);
for (i = 0; i < k; i++)
{
x_[i + 1, 1] = x[ix[i]];//待估参数
for (j = 0; j < k; j++)
{
P_[i + 1, j + 1] = P[ix[i] + 1, ix[j] + 1];//系统状态矩阵 k*k
}
}
for (i = 0; i < nv; i++)//nv为观测方程个数，伪距和相位
{
for (j = 0; j < k; j++)
{
H_[i + 1, j + 1] = H[i + 1, ix[j] + 1]; //观测矩阵 nv*k
}
v_[i + 1, 1] = v[i]; //伪距和相位观测残差 nv*1
R_[i + 1, i + 1] = R[i]; //观测噪声矩阵 nv*nv
}
filter(x_, P_, H_, R_, v_, xp_, Pp_);
for (i = 0; i < k; i++)
{
x[ix[i]] = xp_[i + 1, 1];
for (j = 0; j < k; j++)
{
P[ix[i] + 1, ix[j] + 1] = Pp_[i + 1, j + 1];
}
}
}
/*精密单点定位*/
public static int pppos(ppp_t pppt, List <obs_s> obs, nav_t nav, dcb_t dcb, station sta, erp_t erp)
{
int n = obs.Count, nv = 0, satprn;//当前历元观测的卫星个数
double[][] rs = new double[n][], dts = new double[n][], azel = new double[n][];
double[] var = new double[n], vare = new double[n];
int[] svh = new int[32];
double[] x = new double[nx], v = new double[2 * n], R = new double[2 * n];
matrix P = new matrix(37, 37);//
matrix H = new matrix(2 * n, nx);
for (int i = 0; i < n; i++)
{
rs[i] = new double[6]; //卫星坐标和速度
dts[i] = new double[2]; //卫星的钟差和钟漂
azel[i] = new double[2]; //卫星的方位角和高度角
}
//状态更新
udstate(pppt, dcb, obs);
pppcmn.satpos(obs, nav, sat, clk, pcv, rs, dts, vare, svh);
for (int i = 0; i < nx; i++)
{
x[i] = pppt.x[i];
}
for (int i = 0; i < 10; i++)//滤波迭代
{
if ((nv = res_ppp(obs, sta, pppt, n, rs, dts, azel, vare, erp, dcb, x, R, v, H, svh)) <= 0)
{
break;
}
for (int j = 0; j < nx; j++)
{
for (int k = 0; k < nx; k++)
{
P[j + 1, k + 1] = pppt.P[j + 1, k + 1];
}
}
kalman(x, R, v, P, H, nv);
}
for (int i = 0; i < nx; i++)
{
pppt.x[i] = x[i];
for (int j = 0; j < nx; j++)
{
pppt.P[i + 1, j + 1] = P[i + 1, j + 1];
}
}
for (int i = 0; i < 3; i++)
{
pppt.spp.rr[i] = pppt.x[i];
}
for (int i = 0; i < n; i++)
{
satprn = int.Parse(obs[i].sprn.Substring(1, 2));
if (pppt.vsat[satprn - 1] == 0)
{
continue;
}
pppt.outc[satprn - 1] = 0;
}
return(1);
}
/*矩阵求逆 LUP分解*/
public static matrix inv(matrix a)
{
int n = a.rows;
if (a.rows == a.columns)//是否为方阵
{
matrix b = new matrix(n, n);
matrix A = matrix.matcpy(a);
matrix L = new matrix(n, n);
matrix U = new matrix(n, n);
int[] p = new int[n];
matrix L1 = matrix.eyes(n);
matrix U1 = matrix.eyes(n);
matrix P1 = new matrix(n, n);
#region
#endregion
for (int i = 0; i < n; i++)
{
p[i] = i + 1;//置换矩阵
}
for (int j = 1; j < n; j++)
{
double tmpmax = 0;
int imax = 0, itmp = 0;
for (int i = j; i <= n; i++)
{
if (Math.Abs(A[i, j]) > tmpmax)
{
tmpmax = A[i, j]; imax = i;
}
}
if (tmpmax == 0)
{
return(null);
}
if (imax != j)//交换
{
for (int k = 1; k <= n; k++)
{
tmpmax = A[j, k]; A[j, k] = A[imax, k]; A[imax, k] = tmpmax;
}
itmp = p[j - 1]; p[j - 1] = p[imax - 1]; p[imax - 1] = itmp;
}
for (int i = j + 1; i <= n; i++)//计算LU元素
{
A[i, j] = A[i, j] / A[j, j];
for (int k = j + 1; k <= n; k++)
{
A[i, k] = A[i, k] - A[i, j] * A[j, k];
}
}
}
//
for (int i = 1; i <= n; i++)//分别得到LU
{
L[i, i] = 1;
for (int j = 1; j <= n; j++)
{
if (i > j)
{
L[i, j] = A[i, j];
}
else
{
U[i, j] = A[i, j];
}
}
if (U[i, i] == 0)
{
return(null);
}
}
for (int i = 1; i <= n; i++)//U矩阵化为单位上三角矩阵
{
double t2 = U[i, i];
for (int j = 1; j <= n; j++)
{
U[i, j] = U[i, j] / t2;
U1[i, j] = U1[i, j] / t2;
}
}
for (int j = n; j > 1; j--)//U1，外循环从最后列开始
{
for (int i = j - 1; i >= 1; i--)
{
for (int k = 1; k <= n; k++)
{
U1[i, k] = U1[i, k] - U[i, j] * U1[j, k];
}
}
}
for (int j = 1; j < n; j++)//L的逆，外循环从第一列开始
{
for (int i = j + 1; i <= n; i++)
{
for (int k = 1; k <= n; k++)
{
L1[i, k] = L1[i, k] - L[i, j] * L1[j, k];
}
}
}
for (int i = 1; i <= n; i++)//P1
{
P1[i, p[i - 1]] = 1;
}
b = U1 * L1 * P1;
return(b);
}
else
{
return(null);
}
}
