forked from Yuyyyuuu/GNSS-RTK
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrtk.cpp
1528 lines (1348 loc) · 69.1 KB
/
rtk.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include"MyHeadFile.h"
extern ROVERCFGINFO cfginfo;
/*
****************************************************************************
函数名：数据同步函数（文件）
参数：FBas 指向基站数据文件的指针
FRov 指向流动站数据文件的指针
Raw 同步数据存储处（两个站的观测值、星历、参考定位结果）
返回值：数据同步结果的标志 1-数据同步成功 0-数据同步失败 -1-文件数据结束
函数功能：读取并解码两个站对应文件中的数据，并将其进行时间同步
****************************************************************************
*/
int GetSynObsPP(FILE* FBas, FILE* FRov, RAWDAT* Raw)
{
// 将上一历元的两站数据存储好
memcpy(&Raw->BasEpk0, &Raw->BasEpk, sizeof(Raw->BasEpk0));
memcpy(&Raw->RovEpk0, &Raw->RovEpk, sizeof(Raw->RovEpk0));
// 解码流动站数据
static unsigned char Rbuf[MAXRAWLEN];
static int Rlen = 0;
static int Rflag = 0;
while (!feof(FRov))
{
int obtain_byte = fread(Rbuf + Rlen, 1, MAXRAWLEN - Rlen, FRov);
if (obtain_byte < MAXRAWLEN - Rlen) return -1; // 流动站数据文件结束
Rlen = obtain_byte + Rlen;
Rflag = DecodeNovOem7Dat(Rbuf, Rlen, &Raw->RovEpk, Raw->GpsEph, Raw->BdsEph, &Raw->RovPres);
if (Rflag == 1) break; // 解码到了观测值，跳出循环
}
// 求两站时间差值(单位为s)
double dt = GetDiffTime(&Raw->RovEpk.Time, &Raw->BasEpk.Time);
if (fabs(dt) < 0.1) return 1; // 时间同步
else if (dt < 0) return 0; // 流动站数据滞后，无法解算
else // 基站数据滞后，需要读取更后面时刻的基站数据(循环执行直至时间同步为止)
{
static unsigned char Bbuf[MAXRAWLEN];
static int Blen = 0;
static int Bflag = 0;
bool flag = true;
do
{
while (!feof(FBas))
{
int obtain_byte = fread(Bbuf + Blen, 1, MAXRAWLEN - Blen, FBas);
if (obtain_byte < MAXRAWLEN - Blen) return -1; // 基站数据文件结束
Blen = obtain_byte + Blen;
Bflag = DecodeNovOem7Dat(Bbuf, Blen, &Raw->BasEpk, Raw->GpsEph, Raw->BdsEph, &Raw->BasPres);
if (Bflag == 1) break; // 解码到了观测值，跳出循环
}
dt = GetDiffTime(&Raw->RovEpk.Time, &Raw->BasEpk.Time);
if (dt < 0) return 0; // 基站历元数据丢失，无法解算
if (fabs(dt) < 0.1) flag = false;// 时间完成同步
} while (flag);
return 1;
}
}
/*
****************************************************************************
函数名：数据同步函数（实时）
参数：BasSock 基站数据文件的网口
RovSock 流动站数据文件的网口
Raw 同步数据存储处（两个站的观测值、星历、参考定位结果）
返回值：数据同步结果的标志 1-数据同步成功 0-数据同步失败
函数功能：读取并解码两个站对应网口中的数据，并将其进行时间同步
****************************************************************************
*/
int GetSynObsRT(SOCKET& BasSock, SOCKET& RovSock, RAWDAT* Raw)
{
// 将上一历元的两站数据存储好
memcpy(&Raw->BasEpk0, &Raw->BasEpk, sizeof(Raw->BasEpk0));
memcpy(&Raw->RovEpk0, &Raw->RovEpk, sizeof(Raw->RovEpk0));
Sleep(900);
// 解码流动站数据
static unsigned char RBuf[MAXRAWLEN * 2];
static int RlenB = 0; // RBuf的长度（字节数）
unsigned char Rbuf[MAXRAWLEN]; // 中间存储地
int RlenT = 0; // 中间存储地的长度
if ((RlenT = recv(RovSock, (char*)Rbuf, MAXRAWLEN, 0)) > 0)
{
if ((RlenB + RlenT) > MAXRAWLEN * 2) RlenB = 0; // 若本次读入数据加入大BUFF后会超过其最大长度，则先将原先已有的数据舍弃
memcpy(RBuf + RlenB, Rbuf, RlenT);
RlenB = RlenT + RlenB;
DecodeNovOem7Dat(RBuf, RlenB, &Raw->RovEpk, Raw->GpsEph, Raw->BdsEph, &Raw->RovPres);
}
// 解码基站数据
static unsigned char BBuf[MAXRAWLEN * 2];
static int BlenB = 0; // BBuf的长度（字节数）
unsigned char Bbuf[MAXRAWLEN];
int BlenT = 0; // 中间存储地的长度
if ((BlenT = recv(BasSock, (char*)Bbuf, MAXRAWLEN, 0)) > 0)
{
if ((BlenB + BlenT) > MAXRAWLEN * 2) BlenB = 0;
memcpy(BBuf + BlenB, Bbuf, BlenT);
BlenB = BlenT + BlenB;
DecodeNovOem7Dat(BBuf, BlenB, &Raw->BasEpk, Raw->GpsEph, Raw->BdsEph, &Raw->BasPres);
}
// 求两站时间差值(单位为s)
double dt = GetDiffTime(&Raw->RovEpk.Time, &Raw->BasEpk.Time);
if (fabs(dt)<10 && (Raw->RovEpk.Time.SecOfWeek> Raw->RovEpk0.Time.SecOfWeek)) return 1; // 时间同步
else return 0; // 时间不同步
}
/*
******************************************************************
函数名：观测数据有效性标记函数
参数：EpkA 指向上一历元观测数据的指针
EpkB 指向当前历元观测数据的指针
函数功能：利用接收机的质量数据（即连续跟踪时间locktime）
对当前历元的观测数据进行有效性标记
******************************************************************
*/
void MarkValidPre(const EPOCHOBS* EpkA, EPOCHOBS* EpkB)
{
// 标记准则：只有当双频的locktime均通过检验才标记为true
// GPS：L1-0，L2-1 BDS：B1I-0, B3I-1
bool flag[2]; // 是否连续跟踪的标志（locktime）
// 遍历本历元观测数据中的每一颗卫星
for (int i = 0; i < EpkB->SatNum; i++)
{
// 对于本历元的每颗卫星，其两个质量数据的标志均初始化为false
flag[0] = flag[1] = false;
// 寻找本颗卫星在上一历元中的数据
for (int j = 0; j < EpkA->SatNum; j++)
{
if (EpkA->SatObs[j].System == EpkB->SatObs[i].System && EpkA->SatObs[j].Prn == EpkB->SatObs[i].Prn)
{
// 对两个频率的连续跟踪情况进行检验
for (int n = 0; n < 2; n++)
{
if (EpkA->SatObs[i].LockTime[n]==0.0)
{
// 若上一历元的数据是失锁状态，则本历元的连续跟踪时长大于等于6s才视为通过检验
if (EpkB->SatObs[i].LockTime[n] > 6 || EpkB->SatObs[i].LockTime[n] == 6) flag[n] = true;
else flag[n] = false;
}
else
{
double dt = EpkB->SatObs[i].LockTime[n] - EpkA->SatObs[j].LockTime[n];
if (dt > 0 || dt == 0) flag[n] = true;
else flag[n] = false;
}
}
break;
}
else continue;
}
// 进行观测数据的有效性标记：只有双频均通过连续跟踪条件才标记为true
if (EpkB->SatObs[i].Valid == true) // 对单站数据周跳探测结果为真的再进一步利用接收机内部指标标记
{
if (flag[0] == true && flag[1] == true) EpkB->SatObs[i].Valid = true;
else EpkB->SatObs[i].Valid = false;
}
else; // 默认为false
}
}
/*
**************************************************************
函数名：站间单差函数
参数：EpkR 指向流动站观测数据的指针
EpkB 指向基站观测数据的指针
SDObs 单差结果存储处
函数功能：将流动站与基站的观测数据求差,同时将半周标记传递下来
**************************************************************
*/
void FormSDEpochObs(const EPOCHOBS* EpkR, const EPOCHOBS* EpkB, SDEPOCHOBS* SDObs)
{
memset(SDObs->SdSatObs, 0.0, sizeof(SDObs->SdSatObs));
SDObs->Time = EpkR->Time;
int satnum = 0; // 单差观测数据的可用卫星数
// 外层遍历流动站的观测数据
for (int i = 0; i < EpkR->SatNum; i++)
{
// 若接收机的内部质量指标不合格或卫星星历不可用则跳过本颗卫星数据，不做单差处理
if (EpkR->SatObs[i].Valid == false || EpkR->SatPVT[i].Valid == false) continue;
else;
// 内层遍历基站的观测数据
for (int j = 0; j < EpkB->SatNum; j++)
{
// 若接收机的内部质量指标不合格或卫星星历不可用则跳过本颗卫星数据，不做单差处理
if (EpkB->SatObs[j].Valid == false || EpkB->SatPVT[i].Valid == false) continue;
// 若没有问题则进行匹配
else if (EpkB->SatObs[j].System == EpkR->SatObs[i].System && EpkB->SatObs[j].Prn == EpkR->SatObs[i].Prn)
{
SDObs->SdSatObs[satnum].System = EpkR->SatObs[i].System;
SDObs->SdSatObs[satnum].Prn = EpkR->SatObs[i].Prn;
SDObs->SdSatObs[satnum].nRov = i;
SDObs->SdSatObs[satnum].nBas = j;
for (int k = 0; k < 2; k++)
{
// 只有两个站的伪距都有值才做其单差，否则说明数据丢失->做置零处理
if (fabs(EpkR->SatObs[i].p[k]) > 1e-8 && fabs(EpkB->SatObs[j].p[k]) > 1e-8)
{
SDObs->SdSatObs[satnum].dP[k] = EpkR->SatObs[i].p[k] - EpkB->SatObs[j].p[k];
}
else SDObs->SdSatObs[satnum].dP[k] = 0;
// 只有两个站的相位都有值才做其单差，否则说明数据丢失->做置零处理
if (fabs(EpkR->SatObs[i].l[k]) > 1e-8 && fabs(EpkB->SatObs[j].l[k]) > 1e-8)
{
SDObs->SdSatObs[satnum].dL[k] = EpkR->SatObs[i].l[k] - EpkB->SatObs[j].l[k];
}
else SDObs->SdSatObs[satnum].dL[k] = 0;
// 传递半周标记
if (EpkR->SatObs[i].half[k] == 1 && EpkB->SatObs[j].half[k] == 1) SDObs->SdSatObs[satnum].half[k] = 1; // 不存在半周问题
else SDObs->SdSatObs[satnum].half[k] = 0; // 可能存在半周问题
}
satnum++;
break;
}
else continue;
}
}
SDObs->SatNum = satnum;
}
/*
**************************************************************
函数名：周跳探测函数
参数：Obs 指向每个历元的单差观测数据的指针
函数功能：计算每个历元MW、GF组合观测值并用其来进行周跳探测，
然后对每个卫星的单差观测数据进行有效性标记
**************************************************************
*/
void DetectCycleSlip(SDEPOCHOBS* Obs)
{
/* 思路：Obs中的单差组合观测值数组在进入本函数时为上一历元的数据，在函数结束后存放本历元的单
差组合数据，建立缓冲区用于临时存放当前历元的计算结果以便进行历元间求差探测周跳。 */
MWGF sdcomobs[MAXCHANNUM]; // 存放当前历元的单差组合观测值
for (int i = 0; i < Obs->SatNum; i++)
{
// 检查卫星的单差数据是否正常
if (fabs(Obs->SdSatObs[i].dL[0]) < 1e-8 || fabs(Obs->SdSatObs[i].dL[1]) < 1e-8 || fabs(Obs->SdSatObs[i].dP[0]) < 1e-8 || fabs(Obs->SdSatObs[i].dP[1]) < 1e-8)
{
// 若不正常则不满足周跳探测的条件，标记为无效
Obs->SdSatObs[i].Valid = 0;
continue;
}
else; // 卫星的双频伪距、相位单差数据均正常
sdcomobs[i].Sys = Obs->SdSatObs[i].System;
sdcomobs[i].Prn = Obs->SdSatObs[i].Prn;
sdcomobs[i].GF = Obs->SdSatObs[i].dL[0] - Obs->SdSatObs[i].dL[1];
if (sdcomobs[i].Sys == GPS)
{
sdcomobs[i].MW = (FG1_GPS * Obs->SdSatObs[i].dL[0] - FG2_GPS * Obs->SdSatObs[i].dL[1]) / (FG1_GPS - FG2_GPS) -
(FG1_GPS * Obs->SdSatObs[i].dP[0] + FG2_GPS * Obs->SdSatObs[i].dP[1]) / (FG1_GPS + FG2_GPS);
}
else if (sdcomobs[i].Sys == BDS)
{
sdcomobs[i].MW = (FG1_BDS * Obs->SdSatObs[i].dL[0] - FG3_BDS * Obs->SdSatObs[i].dL[1]) / (FG1_BDS - FG3_BDS) -
(FG1_BDS * Obs->SdSatObs[i].dP[0] + FG3_BDS * Obs->SdSatObs[i].dP[1]) / (FG1_BDS + FG3_BDS);
}
sdcomobs[i].n = 1; // 用于MW的平滑计数,此处赋值为1是为容错出现周跳的情况
// 从上个历元的组合数据中查找本颗卫星的GF和MW组合值
for (int j = 0; j < MAXCHANNUM; j++)
{
if (Obs->SdCObs[j].Sys == sdcomobs[i].Sys && Obs->SdCObs[j].Prn == sdcomobs[i].Prn)
{
// 计算GF、MW组合值的历元间差分
double dGF = fabs(sdcomobs[i].GF - Obs->SdCObs[j].GF);
double dMW = fabs(sdcomobs[i].MW - Obs->SdCObs[j].MW);
// 判断是否超限，未超限则未检测出周跳，标记为true
if (dGF < 0.05 && dMW < 3)
{
Obs->SdSatObs[i].Valid = 1;
// 对MW组合值进行平滑
sdcomobs[i].MW = (Obs->SdCObs[j].MW * Obs->SdCObs[j].n + sdcomobs[i].MW) / (Obs->SdCObs[j].n + 1);
sdcomobs[i].n = Obs->SdCObs[j].n + 1; // 更新MW平滑计数
}
else Obs->SdSatObs[i].Valid = 0; // 超限，则标记为false
break;
}
else continue;
}
}
// 将缓冲区的组合观测值的拷贝到Obs里
memcpy(Obs->SdCObs, sdcomobs, sizeof(sdcomobs));
}
/*
*************************************************************************************
函数名：确定基准星
参数：EpkR 指向流动站观测数据的指针
EpkB 指向基站观测数据的指针
SDObs 指向单差数据的指针
DDObs 指向双差数据的指针
函数功能：选取基准星（双系统各一个），将其PRN号与索引存放在双差数据中
返回值：若两个系统的参考星均选取成功则返回true，否则返回false
注意：传入的EpkR、EpkB需要在本函数外提前经过一次SPP来得到卫星位置、高度角等相关数据
*************************************************************************************
*/
bool DetRefSat(const EPOCHOBS* EpkR, const EPOCHOBS* EpkB, SDEPOCHOBS* SDObs, DDCOBS* DDObs)
{
/*
每个卫星系统各选取一颗卫星作为参考星，0=GPS; 1=BDS
条件：1、伪距和载波相位通过周跳探测，没有粗差和周跳以及半周标记
2、卫星星历正常，卫星位置计算成功
3、卫星高度角最大
*/
short sys = 0; // 0=GPS; 1=BDS
double MaxEle[2] = { 0 , 0 }; // 高度角（寻找最大所用）
// 遍历单差数据
for (int i = 0; i < SDObs->SatNum; i++)
{
if (SDObs->SdSatObs[i].Valid == false|| SDObs->SdSatObs[i].half[0]== 0 || SDObs->SdSatObs[i].half[1] == 0) continue; // 未通过周跳探测或半周检查，不可成为参考星
else if (EpkR->SatPVT[SDObs->SdSatObs[i].nRov].Valid == false || EpkB->SatPVT[SDObs->SdSatObs[i].nBas].Valid == false) continue; // 卫星星历异常，不可成为参考星
else;
// 寻找两个系统中高度角最大的卫星
// 确定存储索引
if (SDObs->SdSatObs[i].System == GPS) sys = 0;
else sys = 1;
// 寻找最大高度角,以流动站为准（基站会人为地设置在开阔的好环境下，故主要兼顾流动站）
if (EpkR->SatPVT[SDObs->SdSatObs[i].nRov].Elevation > MaxEle[sys])
{
MaxEle[sys] = EpkR->SatPVT[SDObs->SdSatObs[i].nRov].Elevation;
DDObs->RefPrn[sys] = SDObs->SdSatObs[i].Prn;
DDObs->RefPos[sys] = i;
}
else continue;
}
if (DDObs->RefPos[0] > -1 && DDObs->RefPos[1] > -1) return true;
else return false;
}
/*
*****************************************************************************************
函数名：获取相对定位浮点解
参数：Raw 指向历元总数据的指针
Rov 流动站的SPP结果（用于初始化坐标）
Fres 浮点解结果
函数功能：进行浮点解计算，保存双差浮点解模糊度及其协因数矩阵、基线向量及其精度指标到Fres
返回值：浮点解成功则返回true，失败则返回false
*****************************************************************************************
*/
bool RTKFloat(RAWDAT* Raw, POSRES* Rov, FloatResult* Fres)
{
/* 定义待用变量 */
double BasPos[3]; // 获取基站坐标(使用解码所得准确坐标)
BLHToXYZ(Raw->BasPres.Pos, BasPos, R_WGS84, F_WGS84);
double RovPos[3] = { Rov->Pos[0],Rov->Pos[1] ,Rov->Pos[2] }; // 设置流动站坐标初值(即SPP的结果)
short DDsatnum[2] = { 0 ,0 }; // GPS和BDS两系统各可用双差卫星数 0=GPS, 1=BDS
short totalsatnum = 0; // 总的可用双差卫星数(基准星不计入)
double RovToRefSat[2] = {0,0}; // 流动站到其参考星的几何距离 0=GPS, 1=BDS
double BasToSats[MAXCHANNUM]; // 基站坐标到所有卫星的几何距离（下标与基站原始观测数据的索引一致）
double DDN[MAXCHANNUM * 2]; // 待估双差模糊度
double x[3 + MAXCHANNUM * 2]; // 待估参数的改正数
double B[(4 * MAXCHANNUM) * (3 + MAXCHANNUM * 2)]; // 设计矩阵
double w[4 * MAXCHANNUM]; // 残差阵
double P[(4 * MAXCHANNUM) * (4 * MAXCHANNUM)]; // 权阵
double N_inv[(3 + MAXCHANNUM * 2) * (3 + MAXCHANNUM * 2)]; // 待估参数的协因数阵，定义在外以便后续存储
// 提取参考卫星位置在原始观测数据中的索引 0=GPS, 1=BDS
int RefSatOfRov[2] = { Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[0]].nRov,Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[1]].nRov };
int RefSatOfBas[2] = { Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[0]].nBas,Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[1]].nBas };
// 参考卫星的位置（对于流动站和基站来说，由于时间不一定完全同步,故各自用各自星历所得的卫星位置）
double RefSatPosOfGPSR[3] = { Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[0],Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[1],Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[2] };
double RefSatPosOfBDSR[3] = { Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[0],Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[1],Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[2] };
double RefSatPosOfGPSB[3] = { Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[0],Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[1],Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[2] };
double RefSatPosOfBDSB[3] = { Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[0],Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[1],Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[2] };
/* 变量初始化 */
// 矩阵初始化置零
memset(DDN, 0.0, sizeof(DDN));
memset(x, 0.0, sizeof(x));
memset(B, 0.0, sizeof(B));
memset(w, 0.0, sizeof(w));
memset(P, 0.0, sizeof(P));
memset(BasToSats, 0, sizeof(BasToSats));
// 初始化双差模糊度，下标与所用观测方程的卫星数据顺序一致
int ii=0; // 存储计数
for (int i = 0; i < Raw->SdObs.SatNum; i++)
{
if (i == Raw->DDObs.RefPos[0] || i == Raw->DDObs.RefPos[1]) continue; // 参考星不计入可用双差卫星数
else if (!(Raw->SdObs.SdSatObs[i].Valid == 1 && Raw->SdObs.SdSatObs[i].half[0] == 1 && Raw->SdObs.SdSatObs[i].half[1] == 1)) continue; // 存在粗差和半周则跳过
else; // 本颗卫星可用
// 确定该卫星所属系统
int sys = -1;
if (Raw->SdObs.SdSatObs[i].System == GPS) sys = 0;
else sys = 1;
// 计算双差观测值
double ddp[2] = { Raw->SdObs.SdSatObs[i].dP[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[0], Raw->SdObs.SdSatObs[i].dP[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[1] };
double ddl[2] = { Raw->SdObs.SdSatObs[i].dL[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[0], Raw->SdObs.SdSatObs[i].dL[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[1] };
// 初始化双差模糊度
DDN[ii] = (sys == 0) ? (ddl[0] - ddp[0]) / WL1_GPS : (ddl[0] - ddp[0]) / WL1_BDS;
DDN[ii + 1] = (sys == 0) ? (ddl[1] - ddp[1]) / WL2_GPS : (ddl[1] - ddp[1]) / WL3_BDS;
ii = ii + 2;
}
// 计算基站坐标到所有卫星的几何距离
for (int i = 0; i < Raw->BasEpk.SatNum; i++)
{
BasToSats[i] = sqrt((BasPos[0] - Raw->BasEpk.SatPVT[i].SatPos[0]) * (BasPos[0] - Raw->BasEpk.SatPVT[i].SatPos[0]) + (BasPos[1] - Raw->BasEpk.SatPVT[i].SatPos[1]) * (BasPos[1] - Raw->BasEpk.SatPVT[i].SatPos[1]) + (BasPos[2] - Raw->BasEpk.SatPVT[i].SatPos[2]) * (BasPos[2] - Raw->BasEpk.SatPVT[i].SatPos[2]));
}
/* 进入最小二乘迭代 */
bool flag = true; // 迭代控制器
int count = 0; // 迭代次数
do
{
// 每次迭代时先将相关矩阵先置零，以免数据填充混乱
memset(x, 0, sizeof(x));
memset(B, 0, sizeof(B));
memset(w, 0, sizeof(w));
memset(P, 0, sizeof(P));
memset(N_inv, 0, sizeof(N_inv));
// 每次迭代时可用卫星数归零，以免数据填充混乱
DDsatnum[0] = DDsatnum[1] = 0;
totalsatnum = 0;
// 计算流动站到其参考星的几何距离 0=GPS, 1=BDS
RovToRefSat[0] = sqrt((RovPos[0] - RefSatPosOfGPSR[0]) * (RovPos[0] - RefSatPosOfGPSR[0]) + (RovPos[1] - RefSatPosOfGPSR[1]) * (RovPos[1] - RefSatPosOfGPSR[1]) + (RovPos[2] - RefSatPosOfGPSR[2]) * (RovPos[2] - RefSatPosOfGPSR[2]));
RovToRefSat[1] = sqrt((RovPos[0] - RefSatPosOfBDSR[0]) * (RovPos[0] - RefSatPosOfBDSR[0]) + (RovPos[1] - RefSatPosOfBDSR[1]) * (RovPos[1] - RefSatPosOfBDSR[1]) + (RovPos[2] - RefSatPosOfBDSR[2]) * (RovPos[2] - RefSatPosOfBDSR[2]));
// 遍历单差观测值来填充B、w矩阵，同时统计各系统的可用双差卫星数
for (int i = 0; i < Raw->SdObs.SatNum; i++)
{
if (i == Raw->DDObs.RefPos[0] || i == Raw->DDObs.RefPos[1]) continue; // 参考星不计入可用双差卫星数
else if (!(Raw->SdObs.SdSatObs[i].Valid == 1 && Raw->SdObs.SdSatObs[i].half[0] == 1 && Raw->SdObs.SdSatObs[i].half[1] == 1)) continue; // 存在粗差和半周
else; // 本颗卫星可用
// 当前卫星的位置（在流动站原始观测数据中的）索引
short satindex = Raw->SdObs.SdSatObs[i].nRov;
// 计算流动站到本颗卫星的几何距离
double RovToSat = sqrt((RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) * (RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) + (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) * (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) + (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]) * (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]));
// 确定本颗卫星所属系统
int sys = -1;
if (Raw->SdObs.SdSatObs[i].System == GPS) sys = 0; // GPS
else sys = 1; // BDS
// 计算B阵的中间量
double l = (RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) / RovToSat - (RovPos[0] - ((sys == 0) ? RefSatPosOfGPSR[0] : RefSatPosOfBDSR[0])) / RovToRefSat[sys];
double m = (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) / RovToSat - (RovPos[1] - ((sys == 0) ? RefSatPosOfGPSR[1] : RefSatPosOfBDSR[1])) / RovToRefSat[sys];
double n = (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]) / RovToSat - (RovPos[2] - ((sys == 0) ? RefSatPosOfGPSR[2] : RefSatPosOfBDSR[2])) / RovToRefSat[sys];
// 填充B阵的四行数据
for (int j = 0; j < 4; j++)
{
B[(3 + MAXCHANNUM * 2) * (4 * totalsatnum + j)] = l;
B[(3 + MAXCHANNUM * 2) * (4 * totalsatnum + j) + 1] = m;
B[(3 + MAXCHANNUM * 2) * (4 * totalsatnum + j) + 2] = n;
if (j == 2) B[(3 + MAXCHANNUM * 2) * (4 * totalsatnum + j) + 3 + 2 * totalsatnum] = (sys == 0) ? WL1_GPS : WL1_BDS;
else if (j == 3) B[(3 + MAXCHANNUM * 2) * (4 * totalsatnum + j) + 4 + 2 * totalsatnum] = (sys == 0) ? WL2_GPS : WL3_BDS;
else continue;
}
// 计算双差观测值
double ddp[2] = { Raw->SdObs.SdSatObs[i].dP[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[0], Raw->SdObs.SdSatObs[i].dP[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[1] };
double ddl[2] = { Raw->SdObs.SdSatObs[i].dL[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[0], Raw->SdObs.SdSatObs[i].dL[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[1] };
double ddrou = RovToSat - RovToRefSat[sys] + BasToSats[RefSatOfBas[sys]] - BasToSats[Raw->SdObs.SdSatObs[i].nBas];
// 填充w阵的四行数据
w[4 * totalsatnum] = ddp[0] - ddrou;
w[4 * totalsatnum + 1] = ddp[1] - ddrou;
w[4 * totalsatnum + 2] = (sys == 0) ? (ddl[0] - ddrou - DDN[totalsatnum * 2] * WL1_GPS) : (ddl[0] - ddrou - DDN[totalsatnum * 2] * WL1_BDS);
w[4 * totalsatnum + 3] = (sys == 0) ? (ddl[1] - ddrou - DDN[totalsatnum * 2 + 1] * WL2_GPS) : (ddl[1] - ddrou - DDN[totalsatnum * 2 + 1] * WL3_BDS);
// 对应系统可用卫星数增加
DDsatnum[sys]++;
totalsatnum = DDsatnum[0] + DDsatnum[1];
}
// 填充P阵,GP和BP是待填充的数值
double GP[4] = { DDsatnum[0] / (DDsatnum[0] + 1.0),1000 * DDsatnum[0] / (DDsatnum[0] + 1.0),-1 / (DDsatnum[0] + 1.0),-1000 / (DDsatnum[0] + 1.0) };
double BP[4] = { DDsatnum[1] / (DDsatnum[1] + 1.0),1000 * DDsatnum[1] / (DDsatnum[1] + 1.0),-1 / (DDsatnum[1] + 1.0),-1000 / (DDsatnum[1] + 1.0) };
// 填充GPS部分
for (int i = 0; i < 4 * DDsatnum[0]; i++)
{
for (int j = 0; j < 4 * DDsatnum[0]; j++)
{
// 填充对角线元素
if (j == i)
{
if (i % 4 == 0 || i % 4 == 1) P[i * (4 * MAXCHANNUM) + j] = GP[0];
else P[i * (4 * MAXCHANNUM) + j] = GP[1];
}
// 填充非对角线元素
else if (j % 4 == i % 4)
{
if (i % 4 == 0 || i % 4 == 1) P[i * (4 * MAXCHANNUM) + j] = GP[2];
else P[i * (4 * MAXCHANNUM) + j] = GP[3];
}
}
}
// 填充BDS部分
int startRow = 4 * DDsatnum[0]; // BDS部分开始的行
int startCol = 4 * DDsatnum[0]; // BDS部分开始的列
for (int i = 0; i < 4 * DDsatnum[1]; i++) {
for (int j = 0; j < 4 * DDsatnum[1]; j++) {
int rowIndex = startRow + i; // 计算行索引
int colIndex = startCol + j; // 计算列索引
// 填充对角线元素
if (i == j) {
if (i % 4 == 0 || i % 4 == 1) P[rowIndex * (4 * MAXCHANNUM) + colIndex] = BP[0];
else P[rowIndex * (4 * MAXCHANNUM) + colIndex] = BP[1];
}
// 填充非对角线元素
else if (i % 4 == j % 4) {
if (i % 4 == 0 || i % 4 == 1) P[rowIndex * (4 * MAXCHANNUM) + colIndex] = BP[2];
else P[rowIndex * (4 * MAXCHANNUM) + colIndex] = BP[3];
}
}
}
// 矩阵重构，使其匹配本项目的矩阵运算函数
restructureMatrix(4 * totalsatnum, 3 + totalsatnum * 2, 4 * MAXCHANNUM, 3 + MAXCHANNUM * 2, B);
restructureMatrix(4 * totalsatnum, 4 * totalsatnum, 4 * MAXCHANNUM, 4 * MAXCHANNUM, P);
// 若双差观测方程数量大于未知数，则可以求解
if (totalsatnum < 5) return false; // 观测不足以解算
else; // 观测足够，进入解算
// 定义并初始化中间计算矩阵
double BT[(3 + MAXCHANNUM * 2) * (4 * MAXCHANNUM)];
MatrixTranspose((4 * totalsatnum), (3 + totalsatnum * 2), B, BT);
double BTP[(3 + MAXCHANNUM * 2) * (4 * MAXCHANNUM)];
memset(BTP, 0.0, sizeof(BTP));
double N[(3 + MAXCHANNUM * 2) * (3 + MAXCHANNUM * 2)];
memset(N, 0.0, sizeof(N));
double W[(3 + MAXCHANNUM * 2) * 1];
memset(W, 0.0, sizeof(W));
// 矩阵运算
MatrixMultiply((3 + totalsatnum * 2), (4 * totalsatnum), (4 * totalsatnum), (4 * totalsatnum), BT, P, BTP);
MatrixMultiply((3 + totalsatnum * 2), (4 * totalsatnum), (4 * totalsatnum), (3 + totalsatnum * 2), BTP, B, N);
int iserror = MatrixInv((3 + totalsatnum * 2), N, N_inv);
if (iserror == 0) return false; // 求逆遇到致命性错误，则退出函数
MatrixMultiply((3 + totalsatnum * 2), (4 * totalsatnum), (4 * totalsatnum), 1, BTP, w, W);
MatrixMultiply((3 + totalsatnum * 2), (3 + totalsatnum * 2), (3 + totalsatnum * 2), 1, N_inv, W, x);
// 更新流动站的位置和双差模糊度参数
RovPos[0] = RovPos[0] + x[0];
RovPos[1] = RovPos[1] + x[1];
RovPos[2] = RovPos[2] + x[2];
for (int i = 0; i < totalsatnum * 2; i++) DDN[i] = DDN[i] + x[i + 3];
// 调整迭代控制器
if ((fabs(x[0]) < 1e-8 && fabs(x[1]) < 1e-8 && fabs(x[2]) < 1e-8) || count > 50) flag = false;
else flag = true;
count++;
}while (flag);
/* 存储结果 */
// 记录所用双差卫星数到浮点解结果和双差结构体内
Raw->DDObs.DDSatNum[0] = Fres->stanum[0] = DDsatnum[0];
Raw->DDObs.DDSatNum[1] = Fres->stanum[1] = DDsatnum[1];
Raw->DDObs.Sats = Fres->totalsatnum = totalsatnum;
// 存储基线向量
Fres->dX[0] = RovPos[0] - BasPos[0];
Fres->dX[1] = RovPos[1] - BasPos[1];
Fres->dX[2] = RovPos[2] - BasPos[2];
// 存储基线向量的DOP
Fres->DOP[0] = sqrt(N_inv[0]);
Fres->DOP[1] = sqrt(N_inv[3 + 2 * totalsatnum + 1]);
Fres->DOP[2] = sqrt(N_inv[2 * (3 + 2 * totalsatnum) + 2]);
Fres->PDOP = sqrt(N_inv[0] + N_inv[3 + 2 * totalsatnum + 1] + N_inv[2 * (3 + 2 * totalsatnum) + 2]);
// 存储双差模糊度
memcpy(Fres->N, DDN, sizeof(double) * 2 * totalsatnum);
// 存储双差模糊度的协因数阵
GetQnn(totalsatnum, N_inv, Fres->Qn );
// 计算验后单位权中误差
// 定义并初始化中间计算矩阵
double V[4 * MAXCHANNUM];
memset(V, 0, sizeof(V));
double Bx[4 * MAXCHANNUM];
memset(Bx, 0, sizeof(Bx));
double VT[4 * MAXCHANNUM];
memset(VT, 0, sizeof(VT));
double VTP[4 * MAXCHANNUM];
memset(VTP, 0, sizeof(VTP));
double VTPV[1] = {0};
// 矩阵运算
MatrixMultiply(4 * totalsatnum, 3 + totalsatnum * 2, 3 + totalsatnum * 2, 1, B, x, Bx);
MatrixSubtraction(4 * totalsatnum, 1, Bx, w, V);
MatrixTranspose(4 * totalsatnum, 1, V, VT);
MatrixMultiply(1, 4 * totalsatnum, 4 * totalsatnum, 4 * totalsatnum, VT, P, VTP);
MatrixMultiply(1, 4 * totalsatnum, 4 * totalsatnum, 1, VTP, V, VTPV);
Fres->sigma = sqrt(VTPV[0] / (2 * totalsatnum - 3));
return true;
}
/*
************************************************************************************
函数名：获取相对定位固定解
参数：Raw 指向历元总数据的指针（此时已包含固定后的整数模糊度）
Rov 流动站的SPP结果（用于初始化坐标）
函数功能：在固定完模糊度的条件下进行只使用相位观测值的解算，得到更精确的定位，
并存储在Raw中的DDObs里的dPos
返回值：固定解成功则返回true，失败则返回false
************************************************************************************
*/
bool RTKFix(RAWDAT* Raw, POSRES* Rov)
{
/* 定义待用变量 */
double BasPos[3]; // 获取基站坐标(使用解码所得准确坐标)
BLHToXYZ(Raw->BasPres.Pos, BasPos, R_WGS84, F_WGS84);
double RovPos[3] = { Rov->Pos[0],Rov->Pos[1] ,Rov->Pos[2] }; // 设置流动站坐标初值(即SPP的结果)
double RovToRefSat[2] = { 0, 0 }; // 流动站到其参考星的几何距离 0=GPS, 1=BDS
double BasToSats[MAXCHANNUM]; // 基站坐标到所有卫星的几何距离（下标与基站原始观测数据的索引一致）
double DDN[MAXCHANNUM * 2]; // 双差模糊度
short DDsatnum[2] = { 0 ,0 }; // GPS和BDS两系统各可用双差卫星数 0=GPS, 1=BDS
short totalsatnum = 0; // 总的可用双差卫星数(基准星不计入)
double x[3]; // 待估参数的改正数
double B[(2 * MAXCHANNUM) * 3]; // B阵
double w[2 * MAXCHANNUM]; // w阵
double P[(2 * MAXCHANNUM) * (2 * MAXCHANNUM)]; // P阵
double N_inv[3 * 3]; // 待估参数的协因数阵，定义在外以便后续存储
// 提取参考卫星位置在原始观测数据中的索引 0=GPS, 1=BDS
short RefSatOfRov[2] = { Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[0]].nRov,Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[1]].nRov };
short RefSatOfBas[2] = { Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[0]].nBas,Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[1]].nBas };
// 参考卫星的位置（对于流动站和基站来说，由于时间不一定完全同步,故各自用各自星历所得的卫星位置）
double RefSatPosOfGPSR[3] = { Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[0],Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[1],Raw->RovEpk.SatPVT[RefSatOfRov[0]].SatPos[2] };
double RefSatPosOfBDSR[3] = { Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[0],Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[1],Raw->RovEpk.SatPVT[RefSatOfRov[1]].SatPos[2] };
double RefSatPosOfGPSB[3] = { Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[0],Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[1],Raw->BasEpk.SatPVT[RefSatOfBas[0]].SatPos[2] };
double RefSatPosOfBDSB[3] = { Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[0],Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[1],Raw->BasEpk.SatPVT[RefSatOfBas[1]].SatPos[2] };
/* 变量初始化 */
// 矩阵初始化置零
memset(x, 0.0, sizeof(x));
memset(B, 0.0, sizeof(B));
memset(w, 0.0, sizeof(w));
memset(P, 0.0, sizeof(P));
memset(BasToSats, 0.0, sizeof(BasToSats));
memset(DDN, 0.0, sizeof(DDN));
// 计算基站坐标到所有卫星的几何距离
for (int i = 0; i < Raw->BasEpk.SatNum; i++)
{
BasToSats[i] = sqrt((BasPos[0] - Raw->BasEpk.SatPVT[i].SatPos[0]) * (BasPos[0] - Raw->BasEpk.SatPVT[i].SatPos[0]) + (BasPos[1] - Raw->BasEpk.SatPVT[i].SatPos[1]) * (BasPos[1] - Raw->BasEpk.SatPVT[i].SatPos[1]) + (BasPos[2] - Raw->BasEpk.SatPVT[i].SatPos[2]) * (BasPos[2] - Raw->BasEpk.SatPVT[i].SatPos[2]));
}
// 提取已固定的双差模糊度，下标与所用观测方程的卫星数据顺序一致
memcpy(DDN, Raw->DDObs.FixedAmb, sizeof(double) * Raw->DDObs.Sats * 2);
/* 最小二乘迭代 */
bool flag = true; // 迭代控制器
int count = 0; // 迭代次数
do
{
// 每次迭代时将相关矩阵先置零，以免数据填充混乱
memset(x, 0, sizeof(x));
memset(B, 0, sizeof(B));
memset(w, 0, sizeof(w));
memset(P, 0, sizeof(P));
memset(N_inv, 0, sizeof(N_inv));
// 每次迭代时可用卫星数归零，以免数据填充混乱
DDsatnum[0] = DDsatnum[1] = 0;
totalsatnum = 0;
// 计算流动站到其参考星的几何距离 0=GPS, 1=BDS
RovToRefSat[0] = sqrt((RovPos[0] - RefSatPosOfGPSR[0]) * (RovPos[0] - RefSatPosOfGPSR[0]) + (RovPos[1] - RefSatPosOfGPSR[1]) * (RovPos[1] - RefSatPosOfGPSR[1]) + (RovPos[2] - RefSatPosOfGPSR[2]) * (RovPos[2] - RefSatPosOfGPSR[2]));
RovToRefSat[1] = sqrt((RovPos[0] - RefSatPosOfBDSR[0]) * (RovPos[0] - RefSatPosOfBDSR[0]) + (RovPos[1] - RefSatPosOfBDSR[1]) * (RovPos[1] - RefSatPosOfBDSR[1]) + (RovPos[2] - RefSatPosOfBDSR[2]) * (RovPos[2] - RefSatPosOfBDSR[2]));
// 遍历单差观测值来填充B、w矩阵，同时统计各系统的可用双差卫星数
for (int i = 0; i < Raw->SdObs.SatNum; i++)
{
if (i == Raw->DDObs.RefPos[0] || i == Raw->DDObs.RefPos[1]) continue; // 参考星不计入可用双差卫星数
else if (!(Raw->SdObs.SdSatObs[i].Valid == 1 && Raw->SdObs.SdSatObs[i].half[0] == 1 && Raw->SdObs.SdSatObs[i].half[1] == 1)) continue; // 存在粗差和半周
else; // 本颗卫星可用
// 当前卫星的位置（在流动站原始观测数据中的）索引
short satindex = Raw->SdObs.SdSatObs[i].nRov;
// 计算流动站到本颗卫星的几何距离
double RovToSat = sqrt((RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) * (RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) + (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) * (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) + (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]) * (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]));
// 确定本颗卫星所属系统
int sys = -1;
if (Raw->SdObs.SdSatObs[i].System == GPS) sys = 0; // GPS
else sys = 1; // BDS
// 计算B阵的中间量
double l = (RovPos[0] - Raw->RovEpk.SatPVT[satindex].SatPos[0]) / RovToSat - (RovPos[0] - ((sys == 0) ? RefSatPosOfGPSR[0] : RefSatPosOfBDSR[0])) / RovToRefSat[sys];
double m = (RovPos[1] - Raw->RovEpk.SatPVT[satindex].SatPos[1]) / RovToSat - (RovPos[1] - ((sys == 0) ? RefSatPosOfGPSR[1] : RefSatPosOfBDSR[1])) / RovToRefSat[sys];
double n = (RovPos[2] - Raw->RovEpk.SatPVT[satindex].SatPos[2]) / RovToSat - (RovPos[2] - ((sys == 0) ? RefSatPosOfGPSR[2] : RefSatPosOfBDSR[2])) / RovToRefSat[sys];
// 填充B阵的两行数据
for (int j = 0; j < 2; j++)
{
B[3 * (2 * totalsatnum + j)] = l;
B[3 * (2 * totalsatnum + j) + 1] = m;
B[3 * (2 * totalsatnum + j) + 2] = n;
}
// 计算双差观测值
double ddp[2] = { Raw->SdObs.SdSatObs[i].dP[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[0], Raw->SdObs.SdSatObs[i].dP[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[1] };
double ddl[2] = { Raw->SdObs.SdSatObs[i].dL[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[0], Raw->SdObs.SdSatObs[i].dL[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[1] };
double ddrou = RovToSat - RovToRefSat[sys] + BasToSats[RefSatOfBas[sys]] - BasToSats[Raw->SdObs.SdSatObs[i].nBas];
// 填充w阵的两行数据
w[2 * totalsatnum] = (sys == 0) ? (ddl[0] - ddrou - DDN[totalsatnum * 2] * WL1_GPS) : (ddl[0] - ddrou - DDN[totalsatnum * 2] * WL1_BDS);
w[2 * totalsatnum + 1] = (sys == 0) ? (ddl[1] - ddrou - DDN[totalsatnum * 2 + 1] * WL2_GPS) : (ddl[1] - ddrou - DDN[totalsatnum * 2 + 1] * WL3_BDS);
// 对应系统可用卫星数增加
DDsatnum[sys]++;
totalsatnum = DDsatnum[0] + DDsatnum[1];
}
// 填充P阵,GP和BP是待填充的数值
double GP[2] = { DDsatnum[0] / (DDsatnum[0] + 1.0),-1.0/ (DDsatnum[0] + 1.0)};
double BP[2] = { DDsatnum[1] / (DDsatnum[1] + 1.0),-1.0/ (DDsatnum[1] + 1.0)};
// 填充GPS部分
for (int i = 0; i < 2 * DDsatnum[0]; i++)
{
for (int j = 0; j < 2 * DDsatnum[0]; j++)
{
// 填充对角线元素
if (j == i)
{
P[i * (2 * MAXCHANNUM) + j] = GP[0];
}
// 填充非对角线元素
else if (j % 2 == i % 2)
{
P[i * (2 * MAXCHANNUM) + j] = GP[1];
}
}
}
// 填充BDS部分
int startRow = 2 * DDsatnum[0]; // BDS部分开始的行
int startCol = 2 * DDsatnum[0]; // BDS部分开始的列
for (int i = 0; i < 2 * DDsatnum[1]; i++)
{
for (int j = 0; j < 2 * DDsatnum[1]; j++)
{
int rowIndex = startRow + i; // 计算行索引
int colIndex = startCol + j; // 计算列索引
// 填充对角线元素
if (i == j)
{
P[rowIndex * (2 * MAXCHANNUM) + colIndex] = BP[0];
}
// 填充非对角线元素
else if (i % 2 == j % 2)
{
P[rowIndex * (2 * MAXCHANNUM) + colIndex] = BP[1];
}
}
}
// 矩阵重构，使其匹配本项目的矩阵运算函数
restructureMatrix(2 * totalsatnum, 2 * totalsatnum, 2 * MAXCHANNUM, 2 * MAXCHANNUM, P);
// 由于浮点解可行，则固定时无需考虑卫星个数是否足够的问题
// 定义并初始化中间计算矩阵
double BT[3 * (2 * MAXCHANNUM)];
MatrixTranspose((2 * totalsatnum),3, B, BT);
double BTP[3 * (2 * MAXCHANNUM)];
memset(BTP, 0.0, sizeof(BTP));
double N[3 * 3];
memset(N, 0.0, sizeof(N));
double W[3 * 1];
memset(W, 0.0, sizeof(W));
// 矩阵运算
MatrixMultiply(3, (2 * totalsatnum), (2 * totalsatnum), (2 * totalsatnum), BT, P, BTP);
MatrixMultiply(3, (2 * totalsatnum), (2 * totalsatnum), 3, BTP, B, N);
int iserror = MatrixInv(3, N, N_inv);
if (iserror == 0) return false; // 求逆遇到致命性错误，则退出函数
MatrixMultiply(3, (2 * totalsatnum), (2 * totalsatnum), 1, BTP, w, W);
MatrixMultiply(3, 3, 3, 1, N_inv, W, x);
// 更新流动站的位置
RovPos[0] = RovPos[0] + x[0];
RovPos[1] = RovPos[1] + x[1];
RovPos[2] = RovPos[2] + x[2];
Raw->DDObs.PDOP = sqrt(N_inv[0] + N_inv[4] + N_inv[8]);
// 调整迭代控制器
if (fabs(x[0]) < 1e-8 && fabs(x[1]) < 1e-8 && fabs(x[2]) < 1e-8 || count > 50) flag = false;
else flag = true;
count++;
} while (flag);
/* 存储结果 */
// 存储固定解的基线向量
Raw->DDObs.dPos[0] = RovPos[0] - BasPos[0];
Raw->DDObs.dPos[1] = RovPos[1] - BasPos[1];
Raw->DDObs.dPos[2] = RovPos[2] - BasPos[2];
// 计算残差平方和
// 定义并初始化中间计算矩阵
double V[2 * MAXCHANNUM];
memset(V, 0, sizeof(V));
double Bx[2 * MAXCHANNUM];
memset(Bx, 0, sizeof(Bx));
double VT[2 * MAXCHANNUM];
memset(VT, 0, sizeof(VT));
double VTP[2 * MAXCHANNUM];
memset(VTP, 0, sizeof(VTP));
double VTPV[1] = { 0 };
// 矩阵运算
MatrixMultiply(2 * totalsatnum, 3, 3 , 1, B, x, Bx);
MatrixSubtraction(2 * totalsatnum, 1, Bx, w, V);
MatrixTranspose(2 * totalsatnum, 1, V, VT);
MatrixMultiply(1, 2 * totalsatnum, 2 * totalsatnum, 2 * totalsatnum, VT, P, VTP);
MatrixMultiply(1, 2 * totalsatnum, 2 * totalsatnum, 1, VTP, V, VTPV);
Raw->DDObs.FixRMS = VTPV[0];
return true;
}
/*
***********************************************************
函数名：滤波初始化函数
参数：Raw 指向历元总数据的指针
Rov 流动站的SPP结果
ekf 滤波数据
函数功能：初始化滤波的状态量X和其对应的方差协方差矩阵P
***********************************************************
*/
void InitFilter(RAWDAT* Raw, POSRES* Rov, RTKEKF* ekf)
{
// 状态清零
memset(&ekf->X0, 0, sizeof(ekf->X0));
memset(&ekf->P0, 0, sizeof(ekf->P0));
memset(&ekf->X, 0, sizeof(ekf->X));
memset(&ekf->P, 0, sizeof(ekf->P));
// 初始化时间标记
memcpy(&ekf->Time, &Raw->RovEpk.Time, sizeof(ekf->Time));
// 初始化状态的位置元素(使用SPP结果填充X的前三个元素)
ekf->X0[0] = Rov->Pos[0];
ekf->X0[1] = Rov->Pos[1];
ekf->X0[2] = Rov->Pos[2];
// 初始化状态的双差模糊度元素（遍历单差数据，按顺序计算并填充）
int ii = 3; // 存储索引 状态X的元素个数为ii --> 为填充P阵提供依据
for (int i = 0; i < Raw->SdObs.SatNum; i++)
{
// 参考星不计入可用双差卫星数
if (i == Raw->DDObs.RefPos[0] || i == Raw->DDObs.RefPos[1])
{
ekf->Index[i] = -1;
continue;
}
else;
// 判断是否存在粗差和半周
if (Raw->SdObs.SdSatObs[i].Valid == 1 && Raw->SdObs.SdSatObs[i].half[0] == 1 && Raw->SdObs.SdSatObs[i].half[1] == 1)
{
ekf->Index[i] = ii; // 本颗卫星对应站间单差数据产生的双频双差模糊度存放在状态X中的索引为ii、ii+1
// 确定该颗卫星所属系统
int sys = -1;
if (Raw->SdObs.SdSatObs[i].System == GPS) sys = 0;
else sys = 1;
// 计算双差观测值并进行模糊度初始化
double ddp[2] = { Raw->SdObs.SdSatObs[i].dP[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[0], Raw->SdObs.SdSatObs[i].dP[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dP[1] };
double ddl[2] = { Raw->SdObs.SdSatObs[i].dL[0] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[0], Raw->SdObs.SdSatObs[i].dL[1] - Raw->SdObs.SdSatObs[Raw->DDObs.RefPos[sys]].dL[1] };
ekf->X0[ii] = (sys == 0) ? ((ddl[0] - ddp[0]) / WL1_GPS) : ((ddl[0] - ddp[0]) / WL1_BDS);
ekf->X0[ii + 1] = (sys == 0) ? ((ddl[1] - ddp[1]) / WL2_GPS) : ((ddl[1] - ddp[1]) / WL3_BDS);
ii = ii + 2;
}
else
{
ekf->Index[i] = -1;
continue;
}
}
// 初始化状态的方差协方差阵
for (int j = 0; j < ii; j++)
{
if (j < 3) ekf->P0[j + j * ii] = 100;
else ekf->P0[j + j * ii] = 500;
}
// 滤波初始化完成，更新标记
ekf->IsInit = true;
}
/*
******************************************************************
函数名：预测函数
参数：Raw 指向历元总数据的指针
ekf 滤波数据
函数功能：对ekf中的状态及其误差协方差阵进行预测
返回值：预测成功则返回true，否则为false
注意：ekf在进入时应已有上一历元、本历元DDOBS内的参考星信息
以及上一历元的SDOBS单差数据信息
******************************************************************
*/
bool EkfPredict(RAWDAT* Raw, RTKEKF* ekf)
{
// 时间传递
memcpy(&ekf->Time, &Raw->RovEpk.Time, sizeof(ekf->Time));
// 统计当前历元的可用双差卫星数
ekf->CurDDObs.Sats = 0;
for (int i = 0; i < Raw->SdObs.SatNum; i++)
{
if (i == ekf->CurDDObs.RefPos[0] || i == ekf->CurDDObs.RefPos[1]) continue; // 参考星，跳过
else if (!(Raw->SdObs.SdSatObs[i].Valid == 1 && Raw->SdObs.SdSatObs[i].half[0] == 1 && Raw->SdObs.SdSatObs[i].half[1] == 1)) continue; // 有周跳或半周标记，跳过
else ekf->CurDDObs.Sats++; // 可以提供当前历元状态中的双差模糊度
}
// 统计上一历元的可用双差卫星数
ekf->DDObs.Sats = 0;
for (int i = 0; i < ekf->SDObs.SatNum; i++)
{
if (i == ekf->DDObs.RefPos[0] || i == ekf->DDObs.RefPos[1]) continue; // 参考星，跳过
else if (!(ekf->SDObs.SdSatObs[i].Valid == 1 && ekf->SDObs.SdSatObs[i].half[0] == 1 && ekf->SDObs.SdSatObs[i].half[1] == 1)) continue; // 有周跳或半周标记，跳过
else ekf->DDObs.Sats++; // 可以提供当前历元状态中的双差模糊度
}
// 定义状态转移矩阵，预测状态的误差协方差阵并初始置零
double fai[(3 + 2 * MAXCHANNUM) * (3 + 2 * MAXCHANNUM)], Q[(3 + 2 * MAXCHANNUM) * (3 + 2 * MAXCHANNUM)];
memset(fai, 0.0, sizeof(fai));
memset(Q, 0.0, sizeof(Q));
// 填充位置元素对应的fai和Q
for (int i = 0; i < 3; i++)
{
fai[i + i * (3 + 2 * ekf->DDObs.Sats)] = 1; // 位置即上一历元的位置
Q[i + i * (3 + 2 * ekf->CurDDObs.Sats)] = 1e-4 * 1e-4; // 静态，故预测误差很小
}
// 记录双系统的参考星有无发生改变 0-GPS，1-BDS
bool ischange[2] = { false, false };
for (int i = 0; i < 2; i++)
{
if (ekf->CurDDObs.RefPrn[i] != ekf->DDObs.RefPrn[i]) ischange[i] = true; // 记录变化情况
else continue;
}
// 记录本历元的参考星在上一历元中是否可用
bool isvalid[2] = { false ,false };
int refindex[2] = { -1,-1 }; // 本历元参考星在上一历元单差数据中的对应存储位置
for (int j = 0; j < ekf->SDObs.SatNum; j++)
{
if (!(ekf->SDObs.SdSatObs[j].Valid == 1 && ekf->SDObs.SdSatObs[j].half[0] == 1 && ekf->SDObs.SdSatObs[j].half[1] == 1)) continue;
else;
if (ekf->SDObs.SdSatObs[j].System == GPS && ekf->SDObs.SdSatObs[j].Prn == ekf->CurDDObs.RefPrn[0])
{
isvalid[0] = true;
refindex[0] = j;
}
else if (ekf->SDObs.SdSatObs[j].System == BDS && ekf->SDObs.SdSatObs[j].Prn == ekf->CurDDObs.RefPrn[1])
{
isvalid[1] = true;
refindex[1] = j;
}
else continue;
}
int xnum = 3; // 本历元的状态X的对应fai和Q的存放行索引 （列索引由Index提供）
int initIndex[MAXCHANNUM]; // 需要初始化的卫星对应双差模糊度在当前历元Sdobs中的索引
int ii = 0; // initIndex存放所用
memset(initIndex, -1, sizeof(initIndex));