ECL-EKF 地球磁偏角以及磁力计融合相关分析

地球磁场计算器：
https://www.ngdc.noaa.gov/geomag/magfield.shtml
计算磁场所需输入：
位置（经度和纬度）以及海拔高度
日期（年月日）
主要计算结果：
1.Declination (D) 磁偏角 以东为正，单位为度分。
2.Inclination (I) 磁倾角 以下为正，单位为度分。
3.Horizontal Intensity (H) 水平磁场强度，单位为纳特斯拉(nanoTesla)。
4.North Component of H (X) 水平磁场H北向分量，北向为正，单位为纳特斯拉。
5.East Component of H (Y) 水平磁场H东向分量，东向为正，单位为纳特斯拉。
6.Vertical Intensity (Z) 垂直磁场强度，向下为正，单位为纳特斯拉。
7.Total Field (F) 总磁场强度，单位为纳特斯拉。

MATLAB地球磁场模型：
https://ww2.mathworks.cn/help/aeroblks/worldmagneticmodel.html

相关链接：
https://www.ngdc.noaa.gov/geomag/declination.shtml#:~:text=Magnetic declination%2C sometimes called magnetic variation%2C is the,east of true north and negative when west.
磁场单位转换器：
https://mathda.com/convert/zh-CN/magneticfield/nano-tesla-to-gauss?sourceValue=5324.2

ECL-EKF中geo_mag_declination.cpp文件分析：

外部调用接口：

// 获取磁偏角(rad)
float get_mag_declination_radians(float lat, float lon)
{
	return get_table_data(lat, lon, declination_table) * 1e-4f; // declination table stored as 10^-4 radians
}

// 获取磁偏角(deg)
float get_mag_declination_degrees(float lat, float lon)
{
	return math::degrees(get_mag_declination_radians(lat, lon));
}

// 获取磁倾角(rad)
float get_mag_inclination_radians(float lat, float lon)
{
	return get_table_data(lat, lon, inclination_table) * 1e-4f; // inclination table stored as 10^-4 radians
}

// 获取磁倾角(deg)
float get_mag_inclination_degrees(float lat, float lon)
{
	return math::degrees(get_mag_inclination_radians(lat, lon));
}

// 获取磁场强度(Gauss)
float get_mag_strength_gauss(float lat, float lon)
{
	return get_table_data(lat, lon, strength_table) * 1e-4f; // strength table table stored as milli-Gauss * 10
}

// 获取磁场强度(Tesla)
float get_mag_strength_tesla(float lat, float lon)
{
	return get_mag_strength_gauss(lat, lon) * 1e-4f; // 1 Gauss == 0.0001 Tesla
}

核心查表函数

// 输入位置信息 获取表格中的数据
static constexpr float get_table_data(float lat, float lon, const int16_t table[LAT_DIM][LON_DIM])
{
	lat = math::constrain(lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);// 表格经纬度范围
	if (lon > SAMPLING_MAX_LON){
		lon -= 360;
	}
	if (lon < SAMPLING_MIN_LON){
		lon += 360;
	}

	/* round down to nearest sampling resolution */
	float min_lat = floorf(lat / SAMPLING_RES) * SAMPLING_RES;// 计算归一化索引
	float min_lon = floorf(lon / SAMPLING_RES) * SAMPLING_RES;// 计算归一化索引

	/* find index of nearest low sampling point */
	unsigned min_lat_index = get_lookup_table_index(&min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT);
	unsigned min_lon_index = get_lookup_table_index(&min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON);

	const float data_sw = table[min_lat_index][min_lon_index];
	const float data_se = table[min_lat_index][min_lon_index + 1];
	const float data_ne = table[min_lat_index + 1][min_lon_index + 1];
	const float data_nw = table[min_lat_index + 1][min_lon_index];

	/* perform bilinear interpolation on the four grid corners */
	const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
	const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);

	const float data_min = lon_scale * (data_se - data_sw) + data_sw;
	const float data_max = lon_scale * (data_ne - data_nw) + data_nw;

	return lat_scale * (data_max - data_min) + data_min;
}

获取表格中目标点周围数据：

const float data_sw = table[min_lat_index][min_lon_index];
const float data_se = table[min_lat_index][min_lon_index + 1];
const float data_ne = table[min_lat_index + 1][min_lon_index + 1];
const float data_nw = table[min_lat_index + 1][min_lon_index];

这几行代码从表格中获取四个相邻网格点的数据。data_sw、data_se、data_ne 和 data_nw 分别表示西南、东南、东北、西北四个方向的数据点。
双线性插值：

const float lat_scale = constrain((lat - min_lat) / SAMPLING_RES, 0.f, 1.f);
const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.f, 1.f);

计算纬度和经度的插值比例 lat_scale 和 lon_scale。constrain 函数确保这些比例值在 [0, 1] 范围内。
进行第一次插值，得到南北方向上的两个插值结果 data_min 和 data_max：

const float data_min = lon_scale * (data_se - data_sw) + data_sw;
const float data_max = lon_scale * (data_ne - data_nw) + data_nw;

最后，进行第二次插值，得到最终的结果，即根据输入经纬度插值得到的地磁数据：

return lat_scale * (data_max - data_min) + data_min;

三个表格

static constexpr const int16_t declination_table[19][37]
static constexpr const int16_t inclination_table[19][37]
static constexpr const int16_t strength_table[19][37]
分别为磁偏角数组、磁倾角数组以及磁场强度数组。
其中[19]表示纬度从-90_{90度，间隔10度，共19个数据，其中[37]表示从-180}180度，间隔10度，共37个数据。
一些固定参数：

static constexpr float SAMPLING_RES = 10;
static constexpr float SAMPLING_MIN_LAT = -90;
static constexpr float SAMPLING_MAX_LAT = 90;
static constexpr float SAMPLING_MIN_LON = -180;
static constexpr float SAMPLING_MAX_LON = 180;

static constexpr int LAT_DIM = 19;
static constexpr int LON_DIM = 37;