磁场加速度公式

发布网友发布时间：2022-05-31 01:31

共1个回答

热心网友时间：2023-10-03 07:02

一般惯导姿态角的初始值是由加速度数据和磁场数据计算得到的，加速度数据计算得到滚转角和俯仰角，由滚转角，俯仰角以及磁场数据经过计算再得到初始的偏航角。下面是具体的计算和推导过程。

计算过程:
1.计算滚转角和俯仰角
这里我们的导航坐标系为东北天坐标系（ENU），载体坐标系为右、前，上（x,y,z）,姿态角的旋转顺序为：z,x,y(偏航、俯仰，滚转)。

对于加速度计，有以下公式：

导航系到载体系的转换矩阵为。设偏航、俯仰，滚转的初始值分别为， , 。

并且满足：

经过计算得到:

所以：

在推导过程中纯手推容易出错，可以借助matlab计算矩阵，这样不易出错，我在推导这部分时也使用matlab来计算。代码如下：

syms x0
syms y0
syms z0
syms g

x_r_matrix=[1,0,0;0,cos(x0),-sin(x0);0,sin(x0),cos(x0)];
y_r_matrix=[cos(y0),0,sin(y0);0,1,0;-sin(y0),0,cos(y0)];
z_r_matrix=[cos(z0),-sin(z0),0;sin(z0),cos(z0),0;0,0,1];
Cn_b = z_r_matrix*x_r_matrix*y_r_matrix;
Cb_n = Cn_b';
g_n=[0;0;g];
g_b=Cb_n*g_n;