航位推算简记

本文仅介绍航位推算（Dead Reckoning,DR）的简要原理。航位推算基于初始位置，姿态角，速度信息，推算后续的位置。主要可分为两步。
（1）东北天向速度计算
基于偏航角yaw，俯仰角pitch，横滚角roll，可以计算出旋转矩阵
$$R=\left[\begin{array}{ccc}cos(roll)cos(yaw)-sin(pitch)sin(roll)sin(yaw)& -cos(pitch)sin(yaw)& cos(yaw)sin(roll)+cos(roll)sin(pitch)sin(yaw)\\ cos(roll)sin(yaw)+cos(yaw)sin(pitch)sin(roll)& cos(pitch)cos(yaw)& sin(roll)sin(yaw)-cos(roll)cos(yaw)sin(pitch)\\ -cos(pitch)sin(roll)& sin(pitch)& cos(pitch)cos(roll)\end{array}\right]$$
载体速度有右向速度$v_x$，前向速度$v_y$，上向速度$v_z$。基于载体速度和旋转矩阵，可得东向速度
$$v_E=[cos(roll)cos(yaw)-sin(pitch)sin(roll)sin(yaw)]v_x-[cos(pitch)sin(yaw)]v_y+[cos(yaw)sin(roll)+cos(roll)sin(pitch)sin(yaw)]v_z$$
北向速度
$$v_N=[cos(roll)sin(yaw)+cos(yaw)sin(pitch)sin(roll)]v_x+[cos(pitch)cos(yaw)]v_y+[sin(roll)sin(yaw)-cos(roll)cos(yaw)sin(pitch)]v_z$$
天向速度
$$v_U=[-cos(pitch)sin(roll)]v_x+[sin(pitch)]v_y+[cos(pitch)cos(roll)]v_z$$
（2）位置更新
位置更新与惯性导航的位置更新方程一致。纬度更新方程为
$$\dot{lat}=\frac{1}{R_M+h}{v}_N$$
经度更新方程为
$$\dot{lon}=\frac{1}{(R_N+h)cos(lat)}{v}_E$$
高度更新方程为
$$\dot{h}=v_U$$
其中，$R_N$是地球卯酉圈主曲率半径
$$R_N=\frac{R_e}{(1-e^{2}si{n}^2(lat){)}^{\frac{1}{2}}}$$
$R_M$是地球子午圈主曲率半径
$$R_M=\frac{R_e(1-e^2)}{(1-e^{2}si{n}^2(lat){)}^{\frac{3}{2}}}$$
其中，$R_e$为地球赤道半径，$e$为地球的椭圆偏心率。若参考WGS-84坐标系，则$R_e=6378137$，$e^2=0.00669437999014$。
则，在t时间间隔后，纬度更新为：
$$la{t}_i=la{t}_{i-1}+\frac{1}{{R_M}_{i-1}+h_{i-1}}{v_N}_{i}t$$
经度更新为：
$$lo{n}_i=lo{n}_{i-1}+\frac{1}{({R_N}_{i-1}+h_{i-1})cos(la{t}_{i-1})}{v_E}_{i}t$$
高度更新为：
$$h_i=h_{i-1}+{v_U}_{i}t$$
C++示例代码如下

#include <iostream>
#include <cmath>
const double WGS84_Re = 6378137;
const double WGS84_e2 = 0.00669437999014;
const double PI = 3.14159265358979323846;
const double D2R = (PI / 180.0);
const double R2D = (180.0 / PI);

void dead_rockoning(double& lon, double& lat, double& height,double yaw, double pitch, double roll, double forward_speed, double right_speed, double up_speod, double time)
{
    double sin_yaw = std::sin(yaw * D2R);
    double cos_yaw = std::cos(yaw * D2R);
    double sin_pitch = std::sin(pitch * D2R);
    double cos_pitch = std::cos(pitch * D2R);
    double sin_roll = std::sin(roll * D2R);
    double cos_roll = std::cos(roll * D2R);
    double rotation_matrix[3][3];
    rotation_matrix[0][0] = cos_roll * cos_yaw - sin_pitch * sin_roll * sin_yaw;
    rotation_matrix[0][1] = -cos_pitch * sin_yaw;
    rotation_matrix[0][2] = cos_yaw * sin_roll * cos_roll * sin_pitch * sin_yaw;
    rotation_matrix[1][0] = cos_roll * sin_yaw + cos_yaw * sin_pitch * sin_roll;
    rotation_matrix[1][1] = cos_pitch * cos_yaw;
    rotation_matrix[1][2] = sin_roll * sin_yaw - cos_roll * cos_yaw * sin_pitch;
    rotation_matrix[2][0] = -cos_pitch * sin_roll;
    rotation_matrix[2][1] = sin_pitch;
    rotation_matrix[2][2] = cos_pitch * cos_roll;
    double east_speed = rotation_matrix[0][0] * right_speed + rotation_matrix[0][1] * forward_speed + rotation_matrix[0][2] * up_speod;
    double north_speed = rotation_matrix[1][0] * right_speed + rotation_matrix[1][1] * forward_speed + rotation_matrix[1][2] * up_speod;
    double sky_speed = rotation_matrix[2][0] * right_speed + rotation_matrix[2][1] * forward_speed + rotation_matrix[2][2] * up_speod;

    double RNh = (WGS84_Re / std::sqrt(1.0 - WGS84_e2 * std::sin(lat * D2R) * std::sin(lat * D2R)) + height) * std::cos(lat * D2R);
    double RMh= WGS84_Re *(1.0- WGS84_e2)/ (1.0 - WGS84_e2 * std::sin(lat * D2R) * std::sin(lat * D2R))*(std::sqrt(1.0 - WGS84_e2 * std::sin(lat * D2R) * std::sin(lat * D2R))) + height;

    double lat_update = north_speed * time * R2D / RMh;
    double lon_update = east_speed * time * R2D / RMh;
    double height_update = sky_speed * time;

    lon = lon + lon_update;
    lat = lat + lat_update;
    height = height + height_update;
}