PX4姿態解算磁偏補償

PX4磁偏補償

PX4 姿態解算中可以通過參數直接設置磁偏角，如果有GPS也可以通過GPS座標查詢。

// px4 attitude_estimator_q.cpp - update_parameters
float mag_decl_deg = 0.0f;
param_get(_params_handles.mag_decl, &mag_decl_deg);
update_mag_declination(math::radians(mag_decl_deg));

if (_mag_decl_auto && gpos.eph < 20.0f && hrt_elapsed_time(&gpos.timestamp) < 1000000)
 {
    /* set magnetic declination automatically */
	update_mag_declination(math::radians(get_mag_declination(gpos.lat, gpos.lon)));
}

磁偏數據庫

通過GPS座標查詢磁偏角需要首先建立磁偏數據庫，磁偏角查詢
根據px4中設置的座標範圍和劃分精度，建立數據庫，設置如下圖。

/** set this always to the sampling in degrees for the table below */
static constexpr float SAMPLING_RES = 10.0f;
static constexpr float SAMPLING_MIN_LAT	= -60.0f;
static constexpr float SAMPLING_MAX_LAT	= 60.0f;
static constexpr float SAMPLING_MIN_LON	= -180.0f;
static constexpr float SAMPLING_MAX_LON	= 180.0f;

導出的數據庫如下圖，PX4中存儲的是取整後的值，會發現兩個表中有的數會有1度的差別，因爲這個值是緩慢變化的。

磁偏計算

磁偏數據庫建立完成之後就可以根據GPS座標查詢到上圖中對應的網格，PX4中採用雙線性插值，即根據網格四個頂點的磁偏角計算座標所在點的磁偏角。

static float
get_table_data(float lat, float lon, const int8_t table[13][37])
{
	/*
	 * If the values exceed valid ranges, return zero as default
	 * as we have no way of knowing what the closest real value
	 * would be.
	 */
	if (lat < -90.0f || lat > 90.0f ||
	    lon < -180.0f || lon > 180.0f) {
		return 0.0f;
	}

	/* 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.0f, 1.0f);
	const float lon_scale = constrain((lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f);

	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;
}

對應算法如下圖：

雙線性插值，可理解爲兩次插值，分別插值出R1,R2, 再通過R1 R2插值出P.