-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathekf.hpp
222 lines (183 loc) · 33 KB
/
ekf.hpp
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
#include <stdio.h>
#include <string.h>
#include <fstream>
#include "_Matrix.h"
#define PI 3.14159265358979 ////
class DataPara
{
public:
int ReadDataPara(void);
int WriteDataPara(void);
int sysT;
double xGPS;
double yGPS;
double zGPS;
long double yawLeft; //left negative
long double pitchLeft; //up positave
long double yawRight;
long double pitchRight;
int ul;
int vl;
int ur;
int vr;
private:
}data1;
class TriAngle
{
public:
////void LineSolve();
void LineSolve(int i);
double Pan_2_l; //horizontal direction angle with optical axis
double Title_2_l; //vertical direction angle with optical axis
double Pan_2_r;
double Title_2_r;
double pitch_l; //pitch angle with horizontal
double yaw_l; //yaw angle with runway
double pitch_r;
double yaw_r;
double x;
double y;
double z;
}triangle1;
class EKF
{
public:
/*
void stateTransF(void);
void ROIestimate(void);
void covariationPredictP(void);
void filterGainPredictK(void);
void stateUpdateF(void);
void covariationUpdate(void);
*/
void calculateHh(long double left_pan,long double left_tilt,long double right_pan,long double right_tilt,
long double x1,long double y1,long double z1, long double* ul,long double* vl,long double *ur,long double* vr,
_Matrix* H);
//state variable s
long double vx;
long double vy;
long double vz;
long double x;
long double y;
long double z;
//predicted the position of Target
long double ul;
long double vl;
long double ur;
long double vr;
double delt_T;
private:
}ekf;
////camera inier parameters
int left_dx = 18935;
int left_dy = 18936;
int left_u0 = 0;
int left_v0 = 0;
int right_dx = 18935;
int right_dy = 18936;
int right_u0 = 0;
int right_v0 = 0;
//camera position in world coordinate system
double left_xc = 0.0;
double left_yc = 0.0;
double left_zc = 33.33;
double right_xc= 10.77;
double right_yc= 0.0;
double right_zc= 33.33;
double delt_Time = 0;
/*initial parameters*/
double w=0.5; //used to estimate the uav position in the world coordinate
double D = 10.77; // distance between two camera
double FieldView_Pan_l = 1.9364*PI/180.00;//yaw field angle of left camera
double FieldView_Tile_l = 1.4523*PI/180.00;// pitch field angle of left camera
double FieldView_Pan_r = 1.9364*PI/180.00;//yaw field angle of right camera
double FieldView_Tile_r = 1.4523*PI/180.00;// pitch field angle of right camera
double m_PanDrift = 0;
double m_TitleDrift = 0;
double m_PanRightDrift = 0;
double m_TitleRightDrift = 0;
//the angle is according to north and sky
double m_Pan_Left_Calibration = -30.5155922628;
double m_Title_Left_Calibration = -1.11477175698;
double m_Pan_Right_Calibration = -32.8400377433;
double m_Title_Right_Calibration = 0.648704812335;
//calculate the initial pitch and yaw angle of left and right PTU
double Initial_angle_Pan_l = m_Pan_Left_Calibration + m_PanDrift;
double Initial_angle_Tile_l = m_Title_Left_Calibration - m_TitleDrift;
double Initial_angle_Pan_r = m_Pan_Right_Calibration + m_PanRightDrift;
double Initial_angle_Tile_r = m_Title_Right_Calibration - m_TitleRightDrift;
double Left2Right_angle = -121.61578245; //the angle between line that connects two camera and north
////here is the angle according to runway and sky(Y axis and Z axis in the world coordinate)
/*
double delt_Pan_l=Left2Right_angle-Initial_angle_Pan_l+90;
double delt_Tile_l=Initial_angle_Tile_l;
double delt_Pan_r=Left2Right_angle-Initial_angle_Pan_r+90;
double delt_Tile_r=Initial_angle_Tile_r;
*/
double delt_Pan_l =-1.1002;
double delt_Tile_l =-1.1148;
double delt_Pan_r =1.2243;
double delt_Tile_r =0.6487;
/*
void calH2(double left_pan, double left_tilt, double right_pan, double
right_tilt, double x1, double b_y1, double z1, double *ul, double *vl,
double *ur, double *vr)
*/
void calculateHh(double left_pan,double left_tilt,double right_pan,double right_tilt,
double x1,double y1, double z1, double* ul,double* vl,double *ur,double* vr,
_Matrix* H)
{
double h[4][6];
double ekf_ul,ekf_vl,ekf_ur,ekf_vr;
*ul = (18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) + 320;
*vl = (18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) + 240;
*ur = 320 - (18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100));
*vr = (18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) + 240;
printf("ekf.ul !!!! = %lf\n",*ul);
printf("ekf.vl !!!! = %lf\n",*vl);
printf("ekf.ur !!!! = %lf\n",*ur);
printf("ekf.vr !!!! = %lf\n",*vr);
//h[0][0] = (18935*cos(left_pan - 2767291362382745/144115188075855872))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[0][0] = (18935*cos(left_pan - 2767291362382745/144115188075855872))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))*(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
//h[0][1] = - (18935*sin(left_pan - 2767291362382745/144115188075855872))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[0][1] = - (18935*sin(left_pan - 2767291362382745/144115188075855872))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))*(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
//h[0][2] = -(sin(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[0][2] = -(sin(left_tilt - 5607936318665237/288230376151711744)*(18935*x1*cos(left_pan - 2767291362382745/144115188075855872) - 18935*y1*sin(left_pan - 2767291362382745/144115188075855872)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))*(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
h[0][3] = 0;
h[0][4] = 0;
h[0][5] = 0;
//h[1][0] = (18936*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[1][0] = (18936*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))*(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
//h[1][1] = (18936*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[1][1] = (18936*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))* (sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
//h[1][2] = - (18936*cos(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/(！！！sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))！！！^2;
h[1][2] = - (18936*cos(left_tilt - 5607936318665237/288230376151711744))/(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)) - (sin(left_tilt - 5607936318665237/288230376151711744)*(18936*y1*cos(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744) - 18936*cos(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + 18936*x1*sin(left_pan - 2767291362382745/144115188075855872)*sin(left_tilt - 5607936318665237/288230376151711744)))/((sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744))*(sin(left_tilt - 5607936318665237/288230376151711744)*(z1 - 3333/100) + y1*cos(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744) + x1*sin(left_pan - 2767291362382745/144115188075855872)*cos(left_tilt - 5607936318665237/288230376151711744)));
h[1][3] = 0;
h[1][4] = 0;
h[1][5] = 0;
//h[2][0] = (18935*cos(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) + (cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(！！！sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))！！！^2;
h[2][0] = (18935*cos(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) + (cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)));
//h[2][1] = (cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(!!!(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))!!!^2 - (18935*sin(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100));
h[2][1] = (cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))) - (18935*sin(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100));
//h[2][2] = (sin(right_tilt + 6526708726531955/576460752303423488)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(!!!sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))！！！^2;
h[2][2] = (sin(right_tilt + 6526708726531955/576460752303423488)*(18935*y1*sin(right_pan + 6158700809943851/288230376151711744) - 18935*cos(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)));
h[2][3] = 0;
h[2][4] = 0;
h[2][5] = 0;
//h[3][0] = (18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(！！！sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))！！！^2;
h[3][0] = (18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)));
//h[3][1] = (18936*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(！！！sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))！！！^2;
h[3][1] = (18936*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)));
//h[3][2] = - (18936*cos(right_tilt + 6526708726531955/576460752303423488))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (sin(right_tilt + 6526708726531955/576460752303423488)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/(！！！sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))！！！^2;
h[3][2] = - (18936*cos(right_tilt + 6526708726531955/576460752303423488))/(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)) - (sin(right_tilt + 6526708726531955/576460752303423488)*(18936*y1*sin(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) - 18936*cos(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + 18936*sin(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)))/((sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100))*(sin(right_tilt + 6526708726531955/576460752303423488)*(z1 - 3333/100) + y1*cos(right_tilt + 6526708726531955/576460752303423488)*cos(right_pan + 6158700809943851/288230376151711744) + cos(right_tilt + 6526708726531955/576460752303423488)*sin(right_pan + 6158700809943851/288230376151711744)*(x1 - 1077/100)));
h[3][3] = 0;
h[3][4] = 0;
h[3][5] = 0;
for (int i = 0;i < H->m;i++)
{
for (int j = 0;j < H->n;j++)
{
H->write(i,j,h[i][j]);
}
}
}