GPS算法的深入研究（C源代码完整） 牛顿定律起到关键性作用！

彩虹糖

#include
#include
#include
#define C 1.0
double newton(MAT *SAT, VEC *PR, VEC *X0, double s, VEC *X)
{
VEC *X1, *dX, *I, *V;
MAT *H, *Ht, *M, *M1;
VEC *VS;
MAT *SIG, *U, *V2, *Hi;
int i, m;
double n = 1.0, p, gdop = 0.0;
? m = SAT -> m;

? if(m < 4) {
??? H = m_get(m, m);
??? Ht = m_get(m, m);
??? M = m_get(m, m);
??? M1 = m_get(m, m);
??? dX = v_get(m);
??? SIG = m_get(m, m);
??? Hi = m_get(m, m);
??? VS = v_get(m);
??? V2 = m_get(m, m);
? }
? else {
??? H = m_get(m, 4);
??? Ht = m_get(m, 4);
??? M = m_get(4, 4);
??? M1 = m_get(4, 4);
??? dX = v_get(4);
??? SIG = m_get(4, m);
??? Hi = m_get(4, m);
??? VS = v_get(4);
??? V2 = m_get(4, 4);
? }
? I = v_get(m);
? X1 = v_get(4);
? V = v_get(4);
? U = m_get(m, m);

? while(n > s) {

??? for(i = 0; i < m; i  ) {
????? p = sqrt((SAT -> me[0] - X0 -> ve[0]) * (SAT ->  me[0] - X0 -> ve[0])   (SAT -> me[1] - X0 -> ve[1]) *  (SAT -> me[1] - X0 -> ve[1])   (SAT -> me[2] - X0 ->  ve[2]) * (SAT -> me[2] - X0 -> ve[2]));
????? if(m < 4) {
??????? switch(m) {
????????? case 1: H -> me[0] = C;
????????????????? break;
????????? case 2: H -> me[0] = (X0 -> ve[2] - SAT -> me[2]) / p;
????????????????? H -> me[1] = C;
????????????????? break;
????????? case 3: H -> me[0] = (X0 -> ve[1] - SAT -> me[1]) / p;
????????????????? H -> me[1] = (X0 -> ve[2] - SAT -> me[2]) / p;
????????????????? H -> me[2] = C;
????????????????? break;
????????? default: break;
??????? }
????? }
????? else {?
??????? H -> me[0] = (X0 -> ve[0] - SAT -> me[0]) / p;
??????? H -> me[1] = (X0 -> ve[1] - SAT -> me[1]) / p;
??????? H -> me[2] = (X0 -> ve[2] - SAT -> me[2]) / p;
??????? H -> me[3] = C;
????? }
??? }
??? for(i = 0; i < m; i  ) {
????? p = sqrt((SAT -> me[0] - X0 -> ve[0]) * (SAT ->  me[0] - X0 -> ve[0])   (SAT -> me[1] - X0 -> ve[1]) *  (SAT -> me[1] - X0 -> ve[1])   (SAT -> me[2] - X0 ->  ve[2]) * (SAT -> me[2] - X0 -> ve[2]));
????? I -> ve = PR -> ve - (p   C * X0 -> ve[3]);
??? }

??? m_transp(H, Ht);
??? m_mlt(Ht, H, M);
??? m_inverse(M, M1);

??? svd(H, U, V2, VS);
??? m_transp(V2, V2);
??? m_zero(SIG);
??? if(m < 4) {
????? for(i = 0; i < m; i  )
??????? SIG -> me = 1.0 / VS -> ve;
??? }
??? else {
????? for(i = 0; i < 4; i  )
??????? SIG -> me = 1.0 / VS -> ve;
??? }???????
??? m_mlt(SIG, U, Hi);
??? m_mlt(V2, Hi, SIG);
??? mv_mlt(SIG, I, dX);

??? if(m < 4) {
????? v_copy(X0, X1);?
????? switch(m) {
??????? case 1: X1 -> ve[3] = X0 -> ve[3]   dX -> ve[0];
??????????????? break;
??????? case 2: X1 -> ve[2] = X0 -> ve[2]   dX -> ve[0];
??????????????? X1 -> ve[3] = X0 -> ve[3]   dX -> ve[1];
??????????????? break;
??????? case 3: X1 -> ve[1] = X0 -> ve[1]   dX -> ve[0];
??????????????? X1 -> ve[2] = X0 -> ve[2]   dX -> ve[1];
??????????????? X1 -> ve[3] = X0 -> ve[3]   dX -> ve[2];
??????????????? break;
??????? default: break;
????? }
??? }
??? else
????? v_add(X0, dX, X1);???????

??? v_sub(X1, X0, V);
??? n = v_norm2(V);
??? v_copy(X1, X0);
? }

? v_copy(X1, X);
? /* Cacul du GDOP */
? if (m < 4) {
????? for(i = 0; i < m; i  )
??????? gdop  = M1 -> me;
? }?
? else {
????? for(i = 0; i < 4; i  )
??????? gdop  = M1 -> me;
? }???
? gdop = sqrt(fabs(gdop));

? v_free(VS);
? v_free(X1);
? v_free(V);
? v_free(I);
? v_free(dX);
? m_free(V2);
? m_free(U);
? m_free(Hi);
? m_free(SIG);
? m_free(M1);
? m_free(M);
? m_free(Ht);
? m_free(H);
? return(gdop);
}
?

nini-345258

GPS算法的深入研究（C源代码完整） 牛顿定律起到关键性作用！

swustlx86

感谢分享
好好学习

chensk-380482

感谢分享
好好学习