source: ntrip/branches/BNC_LM/bancroft.cpp@ 5699

Last change on this file since 5699 was 2052, checked in by mervart, 15 years ago

* empty log message *

File size: 2.2 KB
Line 
1
2#include <cmath>
3
4#include "bancroft.h"
5#include "bncconst.h"
6
7void bancroft(const Matrix& BBpass, ColumnVector& pos) {
8
9 if (pos.Nrows() != 4) {
10 pos.ReSize(4);
11 }
12 pos = 0.0;
13
14 for (int iter = 1; iter <= 2; iter++) {
15 Matrix BB = BBpass;
16 int mm = BB.Nrows();
17 for (int ii = 1; ii <= mm; ii++) {
18 double xx = BB(ii,1);
19 double yy = BB(ii,2);
20 double traveltime = 0.072;
21 if (iter > 1) {
22 double zz = BB(ii,3);
23 double rho = sqrt( (xx-pos(1)) * (xx-pos(1)) +
24 (yy-pos(2)) * (yy-pos(2)) +
25 (zz-pos(3)) * (zz-pos(3)) );
26 traveltime = rho / t_CST::c;
27 }
28 double angle = traveltime * t_CST::omega;
29 double cosa = cos(angle);
30 double sina = sin(angle);
31 BB(ii,1) = cosa * xx + sina * yy;
32 BB(ii,2) = -sina * xx + cosa * yy;
33 }
34
35 Matrix BBB;
36 if (mm > 4) {
37 SymmetricMatrix hlp; hlp << BB.t() * BB;
38 BBB = hlp.i() * BB.t();
39 }
40 else {
41 BBB = BB.i();
42 }
43 ColumnVector ee(mm); ee = 1.0;
44 ColumnVector alpha(mm); alpha = 0.0;
45 for (int ii = 1; ii <= mm; ii++) {
46 alpha(ii) = lorentz(BB.Row(ii).t(),BB.Row(ii).t())/2.0;
47 }
48 ColumnVector BBBe = BBB * ee;
49 ColumnVector BBBalpha = BBB * alpha;
50 double aa = lorentz(BBBe, BBBe);
51 double bb = lorentz(BBBe, BBBalpha)-1;
52 double cc = lorentz(BBBalpha, BBBalpha);
53 double root = sqrt(bb*bb-aa*cc);
54
55 Matrix hlpPos(4,2);
56 hlpPos.Column(1) = (-bb-root)/aa * BBBe + BBBalpha;
57 hlpPos.Column(2) = (-bb+root)/aa * BBBe + BBBalpha;
58
59 ColumnVector omc(2);
60 for (int pp = 1; pp <= 2; pp++) {
61 hlpPos(4,pp) = -hlpPos(4,pp);
62 omc(pp) = BB(1,4) -
63 sqrt( (BB(1,1)-hlpPos(1,pp)) * (BB(1,1)-hlpPos(1,pp)) +
64 (BB(1,2)-hlpPos(2,pp)) * (BB(1,2)-hlpPos(2,pp)) +
65 (BB(1,3)-hlpPos(3,pp)) * (BB(1,3)-hlpPos(3,pp)) ) -
66 hlpPos(4,pp);
67 }
68 if ( fabs(omc(1)) > fabs(omc(2)) ) {
69 pos = hlpPos.Column(2);
70 }
71 else {
72 pos = hlpPos.Column(1);
73 }
74 }
75}
76
77double lorentz(const ColumnVector& aa, const ColumnVector& bb) {
78 return aa(1)*bb(1) + aa(2)*bb(2) + aa(3)*bb(3) - aa(4)*bb(4);
79}
Note: See TracBrowser for help on using the repository browser.