tsrif.cpp

linux的gps应用

#pragma ident "$Id: tsrif.cpp 185 2006-10-05 18:21:39Z btolman $"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S. 
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software. 
//
//Pursuant to DoD Directive 523024 
//
// DISTRIBUTION STATEMENT A: This software has been approved for public 
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file tsrif.cpp
 * Test class measurement update of SRIFilter
 */

//------------------------------------------------------------------------------------
#include <iostream>
#include <iomanip>

#include "SRIFilter.hpp"
#include "StringUtils.hpp"
#include "random.hpp"
#include "DayTime.hpp"
#include "Stats.hpp"
#include "PolyFit.hpp"
#include "RobustStats.hpp"

using namespace std;
using namespace gpstk;

//------------------------------------------------------------------------------------
// convenience
void LSFunc(Vector<double>& X, Vector<double>& f, Matrix<double>& P);
void doIt(Vector<double>& data, Vector<double>& sol, Matrix<double>& cov);

//------------------------------------------------------------------------------------
// prints inside SRIFilter
bool verbose=false;
// data for the various tests
int test,batch,batchsize,batchlen;
// fake satellites
// (X,Y,Z)(t) = RSV*(st*sp,st*cp,ct)
// where t=Theta+OmegaTheta*t, p=Phi+OmegaPhi*t, s=sin(), c=cos()
double RSV[4]={22.123, 20.984, 21.873, 22.749},
  OmegaPhi[4]={ 0.222,  0.128, -0.209,  0.399},
OmegaTheta[4]={ 0.199, -0.233,  0.212, -0.274},
       Phi[4]={-5.000,  3.209,  0.000, -1.230},
     Theta[4]={ 0.000,  6.123, -1.237,  4.003},
      Bias[4]={18.239, 83.928, -2.489, 45.209},
     Drift[4]={ 0.238, -0.983,  0.703,  0.834};
Matrix<double> P6;          // for test 6,7
Vector<double> D6;          // for test 6,7
Vector<double> D4;          // for test 4,5
// test 8,9
// n is the number of turns
// w is the wire width (microm)
// d is the inner diameter (microm)
// D is the outer diameter (microm)
// L is the measured inductance (nH)
double n8[50] = {
    3.7500, 4.2500, 9.2500, 4.5000, 5.7500, 12.0000,  7.0000, 6.0000, 8.0000, 4.0000,
   11.0000, 8.0000, 6.0000, 3.0000, 5.0000,  9.0000, 16.0000, 6.0000, 3.0000, 4.0000,
    2.0000, 2.0000, 3.0000, 9.2500, 4.0000,  8.0000,  6.0000, 3.5000, 4.5000, 5.5000,
    6.5000, 7.5000, 9.2500, 6.7500, 2.5000,  4.2500,  7.0000, 3.0000, 9.0000, 8.0000,
    5.0000, 9.0000, 3.2500, 4.5000, 3.0000,  5.7500,  3.0000, 4.0000, 6.0000, 8.0000
};
double w8[50] = {
   22.4000, 21.0000,  6.0000, 17.6000, 12.0000,  3.2000, 13.0000, 24.0000,  5.0000,
    5.0000,  9.0000, 14.0000, 19.0000, 19.0000, 24.0000,  6.5000,  5.0000,  9.0000,
    9.0000, 14.0000, 14.0000, 19.0000, 24.0000,  5.2000, 16.0000, 10.0000, 15.0000,
   10.0000, 10.0000, 10.0000, 10.0000, 10.0000,  5.2000, 13.0000, 13.0000, 13.0000,
   15.0000, 20.0000,  6.5000,  6.0000,  7.0000,  7.0000, 25.0000, 23.0000, 18.0000,
    9.5000, 90.0000, 16.0000, 16.0000, 16.0000
};
double d8[50] = {
   165.4500, 132.8500,  64.3500, 221.9000,  82.0500,  57.0000,  34.0000,  42.0000,
   164.0000, 236.0000,  22.0000,  20.0000,  32.0000, 170.0000,  28.0000,  25.0000,
    20.0000, 152.0000, 230.0000, 164.0000, 236.0000, 216.0000, 140.0000,  15.8000,
    38.0000,  80.0000,  75.0000, 177.5000, 115.5000,  75.5000,  44.5000,  20.5000,
    15.8000,  34.0000, 204.0000,  34.0000,  30.0000, 210.0000,   5.0000,  46.0000,
    44.0000,  44.0000, 150.5000,  51.0000, 168.0000,  23.7500, 136.0000,  74.0000,
   100.0000, 136.0000
};
double D8[50] = {
   345, 325, 210, 395, 240, 180, 300, 400, 300, 300,
   300, 300, 300, 300, 300, 230, 300, 300, 300, 300,
   300, 300, 300, 145, 226, 310, 285, 255, 216, 199,
   191, 190, 145, 290, 290, 190, 300, 410, 210, 226,
   154, 250, 340, 300, 300, 190, 700, 262, 392, 532
};
double L8[50] = {
    5.2000,  5.5000, 13.7000, 10.2000,  6.5000, 20.1000,  7.6000,  7.3000, 23.5000,
    9.7000, 17.3000,  9.0000,  5.5000,  3.5000,  3.7000,  9.6000, 36.6000, 12.3000,
    5.2000,  5.9000,  2.4000,  2.1000,  2.8000,  6.4000,  2.1000, 13.9000,  7.2000,
    5.2000,  5.3000,  5.3000,  5.3000,  5.5000,  6.2000,  7.0000,  3.1000,  2.1000,
    7.4000,  4.3000,  7.5000,  9.1000,  2.8000, 12.1000,  3.6000,  3.5000,  3.4000,
    3.4000,  3.9000,  3.1000,  9.8000, 23.8000
};
double x10[5]={ 1.5, 1.5, 1.8, 2.0, 2.5 };
//double x10[5]={ 1.5, 1.5, 1.0, 2.0, 2.5 }; // sats in line -> clear double well
double y10[5]={ 1.5, 2.0, 2.5, 1.75, 1.5 };
// 150 data - actually 149, 2005 is made up
int years11[] = {1856,1857,1858,1859,1860,1861,1862,1863,1864,1865,1866,1867,
   1868,1869,1870,1871,1872,1873,1874,1875,1876,1877,1878,1879,1880,1881,
   1882,1883,1884,1885,1886,1887,1888,1889,1890,1891,1892,1893,1894,1895,
   1896,1897,1898,1899,1900,1901,1902,1903,1904,1905,1906,1907,1908,1909,
   1910,1911,1912,1913,1914,1915,1916,1917,1918,1919,1920,1921,1922,1923,
   1924,1925,1926,1927,1928,1929,1930,1931,1932,1933,1934,1935,1936,1937,
   1938,1939,1940,1941,1942,1943,1944,1945,1946,1947,1948,1949,1950,1951,
   1952,1953,1954,1955,1956,1957,1958,1959,1960,1961,1962,1963,1964,1965,
   1966,1967,1968,1969,1970,1971,1972,1973,1974,1975,1976,1977,1978,1979,
   1980,1981,1982,1983,1984,1985,1986,1987,1988,1989,1990,1991,1992,1993,
   1994,1995,1996,1997,1998,1999,2000,2001,2002,2003,2004,2005
};
double temps11[] = {-0.379,-0.462,-0.416,-0.225,-0.372,-0.404,-0.527,-0.273,
   -0.474,-0.267,-0.223,-0.291,-0.225,-0.305,-0.296,-0.351,-0.266,-0.327,
   -0.376,-0.424,-0.452,-0.211,-0.057,-0.287,-0.296,-0.246,-0.262,-0.318,
   -0.349,-0.350,-0.254,-0.343,-0.311,-0.200,-0.413,-0.353,-0.407,-0.449,
   -0.414,-0.362,-0.199,-0.186,-0.339,-0.250,-0.192,-0.256,-0.349,-0.445,
   -0.443,-0.371,-0.292,-0.505,-0.477,-0.447,-0.442,-0.465,-0.406,-0.393,
   -0.248,-0.159,-0.373,-0.495,-0.410,-0.292,-0.293,-0.217,-0.323,-0.299,
   -0.347,-0.246,-0.116,-0.218,-0.227,-0.361,-0.152,-0.099,-0.137,-0.239,
   -0.139,-0.172,-0.121,-0.024,0.076,-0.038,-0.083,0.028,-0.020,0.001,
   0.158,0.039,-0.120,-0.105,-0.093,-0.097,-0.210,-0.093,-0.025,0.045,
   -0.170,-0.189,-0.272,-0.006,0.061,0.014,-0.028,0.015,0.008,0.039,-0.233,
   -0.166,-0.084,-0.093,-0.105,0.040,-0.031,-0.187,-0.041,0.093,-0.171,
   -0.123,-0.200,0.057,-0.036,0.067,0.102,0.133,0.019,0.226,0.031,0.014,
   0.096,0.253,0.240,0.163,0.310,0.251,0.116,0.179,0.232,0.373,0.227,0.411,
   0.579,0.340,0.289,0.422,0.474,0.474,0.459,0.5
};

const int M151=72;
string msg1(" (from DDBase T202B ASWA CTRA G11 G14)");
int n151[] = {
   213, 214, 215, 216, 217, 218, 228, 229, 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
};
double y151[] = {
  0.021, -0.012, -0.027, -0.068, -0.024, -0.043, -0.064, -0.023, -0.114, -0.030,
  0.009, 0.095, 0.108, 0.088, 0.126, 0.195, 0.295, 0.305, 0.303, 0.305, 0.310,
  0.312, 0.310, 0.309, 0.316, 0.313, 0.319, 0.321, 0.322, 0.319, 0.320, 0.320,
  0.319, 0.323, 0.326, 0.325, 0.330, 0.328, 0.329, 0.330, 0.339, 0.337, 0.336,
  0.344, 0.343, 0.343, 0.347, 0.346, 0.345, 0.344, 0.339, 0.345, 0.352, 0.349,
  0.355, 0.357, 0.360, 0.360, 0.359, 0.363, 0.361, 0.357, 0.359, 0.362, 0.363,
  0.364, 0.366, 0.366, 0.367, 0.374, 0.374, 0.375
};

const int M152=276;
string msg2(" (from DDBase T202D ASWA CTRA G16 G25)");
int n152[] = {
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
};
double y152[] = {
0.173, 0.189, 0.175, 0.126, 0.044, -0.081, -0.076, -0.079, -0.079, -0.081,
-0.077, -0.079, -0.074, -0.070, -0.073, -0.072, -0.066, -0.067, -0.060, -0.059,
-0.059, -0.054, -0.055, -0.049, -0.052, -0.052, -0.046, -0.044, -0.044, -0.038,
-0.037, -0.033, -0.036, -0.031, -0.040, -0.036, -0.035, -0.031, -0.035, -0.031,
-0.030, -0.031, -0.030, -0.030, -0.028, -0.025, -0.024, -0.026, -0.027, -0.025,
-0.014, -0.014, -0.012, -0.012, -0.011, -0.005, 0.000, -0.004, 0.001, -0.004,
0.002, 0.001, 0.003, 0.004, 0.002, 0.009, 0.012, 0.011, 0.012, 0.012,
0.011, 0.007, 0.009, 0.011, 0.010, 0.012, 0.010, 0.010, 0.017, 0.018,
0.019, 0.026, 0.024, 0.024, 0.031, 0.034, 0.037, 0.038, 0.040, 0.045,
0.047, 0.045, 0.046, 0.046, 0.044, 0.048, 0.048, 0.046, 0.046, 0.049,
0.046, 0.047, 0.050, 0.044, 0.047, 0.049, 0.050, 0.055, 0.057, 0.056,
0.056, 0.064, 0.066, 0.072, 0.072, 0.072, 0.071, 0.073, 0.075, 0.075,
0.077, 0.076, 0.072, 0.071, 0.081, 0.081, 0.082, 0.086, 0.086, 0.088,
0.083, 0.090, 0.089, 0.094, 0.091, 0.092, 0.095, 0.097, 0.093, 0.095,
0.098, 0.098, 0.099, 0.098, 0.098, 0.103, 0.100, 0.102, 0.101, 0.106,
0.104, 0.107, 0.108, 0.112, 0.112, 0.112, 0.116, 0.117, 0.118, 0.119,
0.125, 0.124, 0.127, 0.127, 0.126, 0.127, 0.127, 0.128, 0.128, 0.132,
0.131, 0.135, 0.131, 0.135, 0.130, 0.131, 0.135, 0.134, 0.136, 0.139,
0.140, 0.140, 0.140, 0.144, 0.145, 0.147, 0.147, 0.149, 0.152, 0.150,
0.151, 0.151, 0.154, 0.154, 0.157, 0.161, 0.163, 0.163, 0.161, 0.162,
0.164, 0.165, 0.165, 0.167, 0.166, 0.166, 0.169, 0.170, 0.171, 0.170,
0.173, 0.172, 0.173, 0.174, 0.175, 0.175, 0.176, 0.176, 0.179, 0.182,
0.184, 0.181, 0.180, 0.183, 0.182, 0.188, 0.187, 0.190, 0.192, 0.194,
0.196, 0.195, 0.196, 0.200, 0.196, 0.201, 0.201, 0.198, 0.199, 0.200,
0.199, 0.201, 0.202, 0.203, 0.201, 0.204, 0.206, 0.209, 0.207, 0.208,
0.210, 0.210, 0.209, 0.212, 0.213, 0.215, 0.211, 0.216, 0.215, 0.214,
0.212, 0.217, 0.216, 0.217, 0.218, 0.219, 0.222, 0.222, 0.223, 0.224,
0.227, 0.227, 0.226, 0.224, 0.226, 0.227
};

const int M153=190;
string msg3(" (from DDBase T202D ASWA CTRA G20 G25)");
int n153[] = {
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
};
double y153[] = {
-0.090, -0.100, -0.084, -0.027, 0.070, 0.202, 0.200, 0.204, 0.203, 0.205,
0.207, 0.208, 0.207, 0.217, 0.212, 0.217, 0.219, 0.221, 0.224, 0.224,
0.229, 0.228, 0.238, 0.235, 0.232, 0.237, 0.239, 0.242, 0.246, 0.243,
0.242, 0.248, 0.243, 0.246, 0.248, 0.254, 0.253, 0.253, 0.253, 0.259,
0.256, 0.259, 0.262, 0.265, 0.267, 0.269, 0.272, 0.274, 0.273, 0.274,
0.276, 0.275, 0.282, 0.282, 0.287, 0.281, 0.289, 0.290, 0.293, 0.297,
0.299, 0.301, 0.298, 0.300, 0.302, 0.306, 0.309, 0.310, 0.310, 0.308,
0.317, 0.316, 0.316, 0.323, 0.326, 0.322, 0.329, 0.327, 0.328, 0.338,
0.336, 0.332, 0.339, 0.345, 0.343, 0.347, 0.350, 0.353, 0.354, 0.355,
0.356, 0.361, 0.364, 0.361, 0.364, 0.368, 0.366, 0.370, 0.372, 0.373,
0.368, 0.374, 0.379, 0.376, 0.374, 0.378, 0.380, 0.384, 0.387, 0.388,
0.389, 0.394, 0.396, 0.395, 0.399, 0.399, 0.398, 0.400, 0.404, 0.408,
0.411, 0.413, 0.415, 0.413, 0.418, 0.419, 0.420, 0.424, 0.428, 0.429,
0.429, 0.429, 0.438, 0.435, 0.438, 0.439, 0.441, 0.440, 0.439, 0.447,
0.448, 0.452, 0.452, 0.454, 0.456, 0.458, 0.461, 0.462, 0.461, 0.465,
0.468, 0.470, 0.469, 0.471, 0.471, 0.478, 0.476, 0.480, 0.481, 0.485,
0.482, 0.492, 0.493, 0.495, 0.493, 0.491, 0.496, 0.501, 0.503, 0.504,
0.509, 0.509, 0.510, 0.510, 0.512, 0.515, 0.516, 0.514, 0.521, 0.520,
0.523, 0.524, 0.527, 0.526, 0.532, 0.533, 0.535, 0.540, 0.537, 0.541
};

const int M154=61;
string msg4(" (from DDBase T202B ASWA CTRA G11 G14 after edit-resets)");
int n154[] = {
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
};
double y154[] = {
0.095, 0.108, 0.088, 0.126, 0.195, 0.295, 0.305, 0.303, 0.305, 0.310,
0.312, 0.310, 0.309, 0.316, 0.313, 0.319, 0.321, 0.322, 0.319, 0.320,
0.320, 0.319, 0.323, 0.326, 0.325, 0.330, 0.328, 0.329, 0.330, 0.339,
0.337, 0.336, 0.344, 0.343, 0.343, 0.347, 0.346, 0.345, 0.344, 0.339,
0.345, 0.352, 0.349, 0.355, 0.357, 0.360, 0.360, 0.359, 0.363, 0.361,
0.357, 0.359, 0.362, 0.363, 0.364, 0.366, 0.366, 0.367, 0.374, 0.374,
0.375
};

int *n15,M15;
double *y15,w15[M152];
string msg;

// formats
#include "format.hpp"
format f103(10,3),f103e(10,3,2),f106(10,6),f133(13,3),f63(6,3);

// stuff for doIt
Vector<double> truth;
SRIFilter srif;

//------------------------------------------------------------------------------------
// test least squares using SRIFilter
int main(int argc, char **argv)
{
try {
   int iterationsLimit = 20;
   double convergenceLimit = 1.e-14;
   int i,j,k,M,N,dataset=2;
   double t,inputsig=-1.,inputtau=-1.;
   Vector<double> data;
   Vector<double> sol;
   Matrix<double> cov,partials;
   Namelist NL;

   if(argc > 1) {
      for(i=1; i<argc; i++) {
         //cout << "arg " << i << " is " << argv[i] << endl;
         j = atoi(argv[i]);
         if(j > 0)
            test = j;
         else if(argv[i][0] == '-') {
            if(argv[i][1] == 'v' || argv[i][1]=='V')
               verbose = true;
            else if(argv[i][1] == 't' || argv[i][1] == 'T') {
               i++;
               inputtau = atof(argv[i]);
            }
            else if(argv[i][1] == 's' || argv[i][1] == 'S') {
               i++;
               inputsig = atof(argv[i]);
            }
            else if(argv[i][1] == 'd' || argv[i][1] == 'D') {
               i++;
               dataset = atoi(argv[i]);
            }
         }
         else
            cout << "Unrecognized option: " << argv[i] << endl;
      }