src/utils/declination.py

   1 #!/usr/bin/env python3
   2 '''
   3 generate field tables from IGRF13. Note that this requires python3
   4
   5 To run the generator you need the igrf module. Install like this:
   6
   7     python3 -m pip install --user igrf
   8
   9 And run:
  10
  11     python3 src/utils/declination.py
  12
  13 It will updates the navigation_declination_gen.c code
  14 '''
  15
  16 import igrf
  17 import numpy as np
  18 import datetime
  19 import pathlib
  20 from math import sin, cos, sqrt
  21 import math
  22
  23 class Vector3(object):
  24     '''a vector'''
  25     def __init__(self, x=None, y=None, z=None):
  26         if x is not None and y is not None and z is not None:
  27             self.x = float(x)
  28             self.y = float(y)
  29             self.z = float(z)
  30         elif x is not None and len(x) == 3:
  31             self.x = float(x[0])
  32             self.y = float(x[1])
  33             self.z = float(x[2])
  34         elif x is not None:
  35             raise ValueError('bad initializer')
  36         else:
  37             self.x = float(0)
  38             self.y = float(0)
  39             self.z = float(0)
  40
  41     def length(self):
  42         return sqrt(self.x**2 + self.y**2 + self.z**2)
  43
  44     def __sub__(self, other):
  45         return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)
  46
  47     def __mul__(self, other):
  48         if isinstance(other, (int, float)):
  49             return Vector3(self.x * other, self.y * other, self.z * other)
  50         elif isinstance(other, Vector3):
  51             return self.x * other.x + self.y * other.y + self.z * other.z
  52         else:
  53             raise ValueError('Multiplication with unsupported type')
  54
  55 class Matrix3(object):
  56     '''a 3x3 matrix, intended as a rotation matrix'''
  57     def __init__(self, a=None, b=None, c=None):
  58         if a is not None and b is not None and c is not None:
  59             self.a = a.copy()
  60             self.b = b.copy()
  61             self.c = c.copy()
  62         else:
  63             self.identity()
  64
  65     def identity(self):
  66         self.a = Vector3(1,0,0)
  67         self.b = Vector3(0,1,0)
  68         self.c = Vector3(0,0,1)
  69
  70     def from_euler(self, roll, pitch, yaw):
  71         '''fill the matrix from Euler angles in radians'''
  72         cp = cos(pitch)
  73         sp = sin(pitch)
  74         sr = sin(roll)
  75         cr = cos(roll)
  76         sy = sin(yaw)
  77         cy = cos(yaw)
  78
  79         self.a.x = cp * cy
  80         self.a.y = (sr * sp * cy) - (cr * sy)
  81         self.a.z = (cr * sp * cy) + (sr * sy)
  82         self.b.x = cp * sy
  83         self.b.y = (sr * sp * sy) + (cr * cy)
  84         self.b.z = (cr * sp * sy) - (sr * cy)
  85         self.c.x = -sp
  86         self.c.y = sr * cp
  87         self.c.z = cr * cp
  88
  89     def __mul__(self, vector):
  90         if isinstance(vector, Vector3):
  91             x = self.a.x * vector.x + self.a.y * vector.y + self.a.z * vector.z
  92             y = self.b.x * vector.x + self.b.y * vector.y + self.b.z * vector.z
  93             z = self.c.x * vector.x + self.c.y * vector.y + self.c.z * vector.z
  94             return Vector3(x, y, z)
  95         else:
  96             raise ValueError('Multiplication with unsupported type')
  97
  98 def write_table(f, name, table):
  99     '''write one table'''
 100     f.write("const float %s[%u][%u] = {\n" %
 101                 (name, NUM_LAT, NUM_LON))
 102     for i in range(NUM_LAT):
 103         f.write("    {")
 104         for j in range(NUM_LON):
 105             f.write("%.5ff" % table[i][j])
 106             if j != NUM_LON-1:
 107                 f.write(",")
 108         f.write("}")
 109         if i != NUM_LAT-1:
 110             f.write(",")
 111         f.write("\n")
 112     f.write("};\n\n")
 113
 114 date = datetime.datetime.now()
 115
 116 SAMPLING_RES = 10
 117 SAMPLING_MIN_LAT = -90
 118 SAMPLING_MAX_LAT = 90
 119 SAMPLING_MIN_LON = -180
 120 SAMPLING_MAX_LON = 180
 121
 122 lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
 123 lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
 124
 125 NUM_LAT = lats.size
 126 NUM_LON = lons.size
 127
 128 intensity_table = np.empty((NUM_LAT, NUM_LON))
 129 inclination_table = np.empty((NUM_LAT, NUM_LON))
 130 declination_table = np.empty((NUM_LAT, NUM_LON))
 131
 132 max_error = 0
 133 max_error_pos = None
 134 max_error_field = None
 135
 136 def get_igrf(lat, lon):
 137     '''return field as [declination_deg, inclination_deg, intensity_gauss]'''
 138     mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
 139     intensity = float(mag.total/1e5)
 140     inclination = float(mag.incl)
 141     declination = float(mag.decl)
 142     return [declination, inclination, intensity]
 143
 144 def interpolate_table(table, latitude_deg, longitude_deg):
 145     '''interpolate inside a table for a given lat/lon in degrees'''
 146     # round down to nearest sampling resolution
 147     min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
 148     min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)
 149
 150     # find index of nearest low sampling point
 151     min_lat_index = int(math.floor((min_lat - SAMPLING_MIN_LAT) / SAMPLING_RES))
 152     min_lon_index = int(math.floor((min_lon - SAMPLING_MIN_LON) / SAMPLING_RES))
 153
 154     # calculate intensity
 155     data_sw = table[min_lat_index][min_lon_index]
 156     data_se = table[min_lat_index][min_lon_index + 1]
 157     data_ne = table[min_lat_index + 1][min_lon_index + 1]
 158     data_nw = table[min_lat_index + 1][min_lon_index]
 159
 160     # perform bilinear interpolation on the four grid corners
 161     data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
 162     data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw
 163
 164     value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
 165     return value
 166
 167 def interpolate_field(latitude_deg, longitude_deg):
 168     '''calculate magnetic field intensity and orientation, interpolating in tables
 169
 170     returns array [declination_deg, inclination_deg, intensity] or None'''
 171     # limit to table bounds
 172     if latitude_deg < SAMPLING_MIN_LAT or latitude_deg >= SAMPLING_MAX_LAT:
 173         return None
 174     if longitude_deg < SAMPLING_MIN_LON or longitude_deg >= SAMPLING_MAX_LON:
 175         return None
 176
 177     intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
 178     declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
 179     inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
 180
 181     return [declination_deg, inclination_deg, intensity_gauss]
 182
 183 def field_to_Vector3(mag):
 184     '''return mGauss field from dec, inc and intensity'''
 185     R = Matrix3()
 186     mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0)
 187     R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0]))
 188     return R * mag_ef
 189
 190 def test_error(lat, lon):
 191     '''check for error from lat, lon'''
 192     global max_error, max_error_pos, max_error_field
 193     mag1 = get_igrf(lat, lon)
 194     mag2 = interpolate_field(lat, lon)
 195     if mag2 is None:
 196         return
 197     ef1 = field_to_Vector3(mag1)
 198     ef2 = field_to_Vector3(mag2)
 199     err = (ef1 - ef2).length()
 200     if err > max_error or err > 100:
 201         print(lat, lon, err, ef1, ef2)
 202         max_error = err
 203         max_error_pos = (lat, lon)
 204         max_error_field = ef1 - ef2
 205
 206 def test_max_error(lat, lon):
 207     '''check for maximum error from lat, lon over SAMPLING_RES range'''
 208     steps = 3
 209     delta = SAMPLING_RES/steps
 210     for i in range(steps):
 211         for j in range(steps):
 212             lat2 = lat + i * delta
 213             lon2 = lon + j * delta
 214             if lat2 > SAMPLING_MAX_LAT or lon2 > SAMPLING_MAX_LON:
 215                 continue
 216             if lat2 < SAMPLING_MIN_LAT or lon2 < SAMPLING_MIN_LON:
 217                 continue
 218             test_error(lat2, lon2)
 219
 220 for i, lat in enumerate(lats):
 221     for j, lon in enumerate(lons):
 222         mag = get_igrf(lat, lon)
 223         declination_table[i][j] = mag[0]
 224         inclination_table[i][j] = mag[1]
 225         intensity_table[i][j] = mag[2]
 226
 227 with open(pathlib.Path(__file__).parent / '..' / 'main' / 'navigation' / 'navigation_declination_gen.c', 'w') as f:
 228     f.write('/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */\n\n\n')
 229     f.write('/* Updated on %s */\n\n\n' % date)
 230
 231     f.write('''const float SAMPLING_RES = %u;
 232 const float SAMPLING_MIN_LAT = %u;
 233 const float SAMPLING_MAX_LAT = %u;
 234 const float SAMPLING_MIN_LON = %u;
 235 const float SAMPLING_MAX_LON = %u;
 236
 237 ''' % (SAMPLING_RES,
 238            SAMPLING_MIN_LAT,
 239            SAMPLING_MAX_LAT,
 240            SAMPLING_MIN_LON,
 241            SAMPLING_MAX_LON))
 242
 243     write_table(f, 'declination_table', declination_table)
 244     write_table(f, 'inclination_table', inclination_table)
 245     write_table(f, 'intensity_table', intensity_table)
 246
 247 print("Checking for maximum error")
 248 for lat in range(-60, 60, 1):
 249     for lon in range(-180, 180, 1):
 250         test_max_error(lat, lon)
 251 print("Generated with max error %.2f %s at (%.2f,%.2f)" % (
 252     max_error, max_error_field, max_error_pos[0], max_error_pos[1]))