-
Notifications
You must be signed in to change notification settings - Fork 0
/
newRAIM1.py
124 lines (102 loc) · 3.05 KB
/
newRAIM1.py
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
import numpy as np
inf = [[-368461.739, 26534822.568, -517664.322, 21966984.2427, -0.000104647296],
[10002180.758, 12040222.131, 21796269.831, 23447022.1136, -0.000308443058],
[-7036480.928, 22592611.906, 11809485.040, 20154521.4618, -0.000038172460],
[8330122.410, 23062955.196, 10138101.718, 22129309.3677, -0.0002393560]]
inf1 = [[-368461.739, 26534822.568, -517664.322],
[10002180.758, 12040222.131, 21796269.831],
[-7036480.928, 22592611.906, 11809485.040],
[8330122.41, 23062955.196, 10138101.718]]
p = [21966984.2427, 23447022.1136, 20154521.4618, 22129309.3677]
t = [-0.000104647296, -0.000308443058, -0.000038172460, -0.000239356]
# 初始估计接收机位置
pos0 = [0, 0, 0]
pos1 = [-2279829.1069, 5004709.2387, 3219779.0559]
# 获取两点之间的距离
def get_distance1(pos1, pos2):
v1 = np.array([pos1[0], pos1[1], pos1[2]])
v2 = np.array([pos2[0], pos2[1], pos2[2]])
distance = np.linalg.norm(v1 - v2)
return distance
# 获取修复钟差的伪距
def get_distance2(pos1, pos2, clk):
v1 = np.array([pos1[0], pos1[1], pos1[2]])
v2 = np.array([pos2[0], pos2[1], pos2[2]])
distance = np.linalg.norm(v1 - v2)
res = distance - (3 * 10 ** 8) * clk
return res
# 获取估计伪距值
def getp():
res = []
i = 0
while i < len(inf1):
res.append(get_distance2(pos0, inf1[i], t[i]))
i = i + 1
return res
# 获取伪距差值
def getdetp(p1):
res = []
k = len(p)
i = 0
while i < k:
res.append(p[i] - p1[i])
i = i + 1
return res
# 获得每一个r
def getdis():
res = []
k = len(p)
i = 0
while i < k:
dis = get_distance1(inf[i], pos0)
res.append(dis)
i = i + 1
return res
# 获取观测矩阵
def getmatH(info, pos, r):
res = []
k = len(info)
i = 0
c = 3 * 10 ** 8
while i < k:
l = []
l.append((pos[0] - info[i][0]) / r[i])
l.append((pos[1] - info[i][1]) / r[i])
l.append((pos[2] - info[i][2]) / r[i])
i = i + 1
res.append(l)
return res
def calresult():
global pos0
for j in range(1000):
p1 = getp()
# print("估计位置到各卫星的伪距值为：\n", p1)
detp = getdetp(p1)
# print("估计伪距和实际伪距的差：\n", detp)
r = getdis()
# print("获取的r为：\n", r)
H = getmatH(inf1, pos0, r)
# print("获得的观测矩阵H为:\n", H)
H1 = np.array(H)
# print(H1)
H2 = np.transpose(H1)
# print(H2)
H3 = np.dot(H2, H1)
# print(H3)
H4 = np.linalg.pinv(H3)
# print(H4)
H5 = np.dot(H4, H2)
# print(H5)
H6 = np.dot(H1, H5)
In = [[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
S = In - H6
R = np.dot(S, detp)
Rt = np.transpose(R)
Ts = np.dot(Rt, R)
print("Ts:", Ts)
detx = np.dot(H5, detp)
# print(detx)
pos0 = pos0 + detx
print(pos0)
print(pos1 - pos0)
calresult()