解析結果

2007 年 2 月 13 日

[figure]

• 2006/06/09 に実施した JAXA MuPAL - α を用いた実験 • 温度補正、取付け補正あり

[figure]

図 1 2D 履歴図 2 高度図 3 北方向速度

2

図 4 東方向速度図 5 重力方向速度

3

図 6 ロール図 7 ピッチ

4

図 8 ヨー図 9 対地速度横滑り角

5

表 1 プロトタイプと GAIA の差異 (GPS Time 3.58E+08[msec] 以降)

平均値標準偏差最悪値
水平距離 [m]  5.37884222304374  2.17047608681524 10.6350963598937
高度 [m]  8.80062791711247  4.49189531057675 18.1365544813652
北方向速度 [m/s] 0.00251701338254768 0.131769439369425  1.4427641514
東方向速度 [m/s] -0.00465839112641328 0.130486115746505  -1.2872197828
重力方向速度 [m/s] -0.326693661917974 0.170983111347295  -0.93187559
ロール [deg]  -0.00907130978994206 0.226962158890542  -0.912784827
ピッチ [deg]  0.105385288014087  1.01461403062255  3.223850122
ヘディング [deg]  1.61262277226337  6.95933536841826  18.95288208

ソース

ソースコード 1 source.cpp

1 /∗
2 ∗
3 ∗/
4 #include <iostream>
5 #include <iomanip>
6 #include <strstream>
7 #include <exception>
8 #include <math.h>
9 #include <time.h>

10

11 #if MSC VER >= 1400
12 #define sprintf sprintf s
13 #endif

14

15 using namespace std;

16

17 typedef double accuracy;

18

19 #include "common .h"

20

21 #include "INS_GPS2 .h"
22 #include "INS_GPS_BE .h"
23 #include "sensor /MEMS .h"

24

25 #include "util /logger .h"

26

27 typedef MEMS Accelerometer mems a;
28 typedef MEMS Gyro mems g;

29

30 //#defineDUMP UPDATE
31 #define DUMP CORRECT

32

33 //#defineBIAS ON
34 //#defineCOV OUT

36 #define DUMP TARGET 9

37 const char ∗label[DUMP TARGET] = {"東経 [秒]", "北緯 [秒]", "高度 [m]", "北方向速度 [m/s]", "東方向

       速度 [m/s]", "重力方向速度 [m/s]", "Ψ [deg]", "Θ [deg]", "Φ [deg]"};

38

39 #ifdef BIAS ON
40 typedef INS GPS2 BiasEstimated<accuracy> NAV;
41 #else
42 typedef INS GPS2<accuracy> NAV;
43 #endif

44

45 class CovObserver : public Loggable{
46 protected :
47  const NAV & m nav;
48 public:
49  CovObserver(const NAV & nav) : m nav(nav){}
50  ˜ CovObserver(){}

51

52  void inspect(ostream & out) const {
53  NAV & nav(∗(const cast <NAV ∗>(& m nav)));

54

55  Matrix<accuracy> P(nav.getFilter().getP());
56  out << scientific<< setprecision(8);
57  for(int i = 0; i < P.rows(); i++){
58  for(int j = i; j < P.columns(); j++){
59  out << P(i, j) << "\t";
60  }
61  }
62  }
63 };

64

65 class Status{
66 private :
67  bool bool init;
68  NAV nav;
69  CovObserver cov;
70  Logger logger;

71

72 public:
73  Status() : bool init(false), nav(), cov(nav), logger("additional .txt"){
74 #ifdef BIAS ON
75  nav.beta accel() ∗= 0.1;
76  nav.beta gyro() ∗= 0.1;//mems g.BETA;
77 #endif

78

79  Matrix<accuracy> P(nav.getFilter().getP());
80  {
81  P(0, 0) = P(1, 1) = P(2, 2) = 1E+1;
82  P(3, 3) = P(4, 4) = P(5, 5) = 1E−8;
83  P(6, 6) = 1E+2;
84  P(7, 7) = P(8, 8) = P(9, 9) = 1E−4; //CRS031E−1; //−4
85 #ifdef BIAS ON
86  P(10, 10) = P(11, 11) = P(12, 12) = 1E−2;
87  P(13, 13) = P(14, 14) = P(15, 15) = 1E−8;
88 #endif
89  }
90  Matrix<accuracy> Q(nav.getFilter().getQ());
91  {

92  Q(0, 0) = pow(ACC STD /ACC 1G ∗ SCALE 1G, 2); //pow(pow(0.01,mems a.WN SLOPE) ∗ mems a.
WN INTERCEPT/mems a.SF, 2);
93  Q(1, 1) = pow(ACC STD /ACC 1G ∗ SCALE 1G, 2); //pow(pow(0.01,mems a.WN SLOPE) ∗ mems a.
WN INTERCEPT/mems a.SF, 2);
94  Q(2, 2) = pow(ACC STD /ACC 1G ∗ SCALE 1G, 2); //pow(pow(0.01,mems a.WN SLOPE) ∗ mems a.
WN INTERCEPT/mems a.SF, 2);
95  Q(3, 3) = pow(deg2rad(GYRO STD /GYRO 1DEG), 2); //pow(pow(0.01,mems g.WN SLOPE) ∗ mems g.
WN INTERCEPT/mems g.SF, 2);
96  Q(4, 4) = pow(deg2rad(GYRO STD /GYRO 1DEG), 2); //pow(pow(0.01,mems g.WN SLOPE) ∗ mems g.
WN INTERCEPT/mems g.SF, 2);
97  Q(5, 5) = pow(deg2rad(GYRO STD /GYRO 1DEG), 2); //pow(pow(0.01,mems g.WN SLOPE) ∗ mems g.
WN INTERCEPT/mems g.SF, 2);
98  Q(6, 6) = 1E−6; //1E−14
99 #ifdef BIAS ON
100  Q(7, 7) = 1E−8; //1E−10;
101  Q(8, 8) = 1E−8; //1E−10;
102  Q(9, 9) = 1E−8; //1E−10;
103  Q(10, 10) = 1E−10; //pow(mems g.ROOT N, 2) ∗ 1. /100 ∗ 16; //sim6 と同じ設定
104  Q(11, 11) = 1E−10; //pow(mems g.ROOT N, 2) ∗ 1. /100 ∗ 16;
105  Q(12, 12) = 1E−10; //pow(mems g.ROOT N, 2) ∗ 1. /100 ∗ 16;
106 #endif
107  }
108  //Q ∗= 2;
109  nav.getFilter().init();
110 #ifdef COV OUT
111  logger.add(cov);
112 #endif
113  }

114

115  NAV & get nav() {return nav;}

116

117  static void dump label(){

118  cout << "longitude " << "\t"
119  << "latitude " << "\t"
120  << "height " << "\t"
121  << "v_north " << "\t"
122  << "v_east " << "\t"
123  << "v_down " << "\t"

124 << "Yaw(Ψ)" << "\t" //Ψ (yaw) 125 << "Pitch (Θ)" << "\t" //Θ (pitch) 126 << "Roll (Φ)" << "\t" //Φ (roll) 127 << "Azimuth (α)" << "\t"; //α (azimuth)

128 #ifdef BIAS ON

129  cout << "bias_accel (X)" << "\t" //Bias
130  << "bias_accel (Y)" << "\t"
131  << "bias_accel (Z)" << "\t"
132  << "bias_gyro (X)" << "\t"
133  << "bias_gyro (Y)" << "\t"
134  << "bias_gyri (Z)" << "\t";

135 #endif
136  }

137

138  void dump(const int & itow){
139  cout << itow << "\t"
140  << setprecision(10)
141  << rad2deg(nav.longitude()) << "\t"
142  << rad2deg(nav.latitude()) << "\t"

143  << nav.height() << "\t"
144  << nav.v north() << "\t"
145  << nav.v east() << "\t"
146  << nav.v down() << "\t"

147 << rad2deg(nav.heading()) << "\t" //Ψ (yaw) <− q {g}ˆ {b} 148 << rad2deg(nav.euler theta()) << "\t" //Θ (pitch) <− q {n}ˆ {b} 149 << rad2deg(nav.euler phi()) << "\t" //Φ (roll) <− q {n}ˆ {b} 150 << rad2deg(nav.azimuth()) << "\t"; //α (azimuth)

151 #ifdef BIAS ON
152  cout << nav.bias accel().getX() << "\t" //Bias
153  << nav.bias accel().getY() << "\t"
154  << nav.bias accel().getZ() << "\t"
155  << nav.bias gyro().getX() << "\t"
156  << nav.bias gyro().getY() << "\t"
157  << nav.bias gyro().getZ() << "\t";
158 #endif
159  logger.out() << itow << "\t";
160  logger.flush();
161  logger.out() << endl;
162  }

163

164  void update(const A Packet & current, const A Packet & next){

165

166 #define pow2(x) ((x) ∗ (x))
167  //cout << current.acc x() << ’ ’ << current.acc y() << ’ ’ << current.acc z() << endl;
168  //cout << current.gyro x() << ’ ’ << current.gyro y() << ’ ’ << current.gyro z() << endl;
169  //cout << sqrt(pow2(current.acc x()) + pow2(current.acc y()) + pow2(current.acc z())) << endl;

170

171  if(!bool init){return ;}

172

173  //cout << ”update: ” << current.itow<< ’ ’ << current.interval(next) << endl;
174  nav.update((current.accel() + next.accel()) /2, (current.gyro() + next.gyro()) /2, INTERVAL UNIT ∗ current.
interval(next));

175

176 #ifdef DUMP UPDATE
177  cout << "U\t";
178  dump(current.itow);
179  cout << endl;
180 #endif
181  }

182

183  void correct(const G Packet & g packet){
184  //cout << g packet.acc 2d << ”,” << g packet.acc v << endl;

185

186  static int count = 0;

187

188  if(bool init){
189  //cout << ”correct: ” << g packet.itow<< endl;

190

191  nav.correct(g packet.convert());
192  //nav.correct(g packet.convert2());

193

194  /∗if(count == 50){
195  nav.initAttitude(deg2rad(−105), nav.euler theta(), nav.euler phi());
196  }∗/
197  count++;

198

200 #ifdef DUMP CORRECT
201  cout << "C\t";
202  dump(g packet.itow);
203  cout << endl;
204 #endif
205  }else if(!bool init && g packet.acc 2d < 100.){
206  bool init = true ;
207  /∗cout << g packet.llh[0] << ’,’
208  << g packet.llh[1] << ’,’
209  << g packet.llh[2] << endl;∗/
210  nav.initPosition(deg2rad(g packet.llh[1]), deg2rad(g packet.llh[0]), g packet.llh[2]);
211  nav.initVelocity(0.,0.,0.);
212  nav.initAttitude(deg2rad(0), deg2rad(0), 0.);
213  }
214  }
215 };

216 217 218

219 int main(){

220

221 Status status;

222

223 cout << "Type \t" << "ITOW (ms)\t";

224 Status::dump label();
225 cout << endl;

226

227 char buf[1024];
228 strstream line(buf, sizeof(buf), strstream::in);

229

230 A Packet ∗a packet current(NULL), ∗a packet next(NULL);
231 G Packet ∗g packet(NULL);

232

233 while(!cin.eof()){
234  cin.getline(buf, sizeof(buf));
235  buf[cin.gcount()] = '\0';
236  //cout << buf<< endl;

237

238  char header;
239  line.seekg(0);

240

241  line >> header;
242  if('A' == header){
243  delete(a packet current);
244  a packet current = a packet next;
245  a packet next = new A Packet(line);
246  if(a packet current){
247  status.update(∗a packet current, ∗a packet next);
248  }
249  }else if('G' == header){
250  try{
251  g packet = new G Packet(line);
252  if(a packet next && (a packet next−> interval(∗g packet) > 5)){continue ;}
253  }catch(exception & e){
254  line.clear();
255  }

256  }

257

258  if(g packet){
259  status.correct(∗g packet);
260  delete(g packet);
261  g packet = NULL;
262  }
263 }

264

265 cerr << "P:" << status.get nav().getFilter().getP() << endl;
266 delete(a packet next);
267 }