Assignments of Inertial Navigation 《惯性导航》作业1
Autumn 2018
Assignment 1: Aircraft Displaying on the Earth
Attached is a group of MA TLAB programs for displaying an aircraft near the surface of the Earth, as shown in figure 1.1 which is drawn by running the program main.m.
If you input the longitude, latitude, heading, pitching, and rolling angles (all in degree), and click the updating button, then the aircraft are expected to be displayed at a proper position on the Earth, and with proper attitude. The coordinates of each vertex of the aircraft 3D model occupies a row in the matrix vtx.
Before you click the updating button for the first time, the aircraft is hidden at the center of the Earth. Please rewrite the program redraw.m by adding the necessary rotation and shifting operations to the vertex coordinates of the aircraft. The acquired position and attitude information has been stored in the variables lon, lat, head, pitch and roll. With each updating, the coordinates of the aircraft in vtx in the MA TLAB drawing frame (same as the Earth frame) need to be re-calculated, instead of being kept the same as their values in the original 3D model (vtx0). That is, in the program redraw.m, you need to replace the command line vtx=vtx0 with your own codes for re-calculating vtx.
And please explain the rationale behind your rewriting.
作业报告
一．理论基础
a. 旋转变换矩阵
在坐标系中，可以通过旋转角度知道一个点在新旧坐标系的坐标 在这样的旋转变化下存在如下的坐标变换矩阵：
[x ′y ′z ′
]=[100
0cosα
sinα0−sinαcosα][x y z
] 同样的，绕y 轴和z 轴旋转的坐标变换矩阵为：
[x ′
y ′z ′
]=[cosβ0−sinβ0
10sinβ0cosβ][x
y z ] [x ′y ′z ′
]=[cosγsinγ0−sinγ
cosγ000
1][x y z ] 其中α、β、γ分别代表绕轴旋转的角度。
b. 平移变换矩阵
为了将飞机平移到给定经纬度的地球表面，需要在旋转之后的坐标上加上地球半径的分量
在球坐标系和直角坐标系中有如下的转换关系：
{x =Rsinθcosφy =Rsinθsinφz =Rcosθ
二．设计思路
程序的整体思路分为两部分，第一部分在初始位置根据经度、维度、航向角、俯仰角和滚转角计算出坐标旋转矩阵，第二部分根据经度纬度计算出坐标平移矩阵，将转换好的飞机平移到平面。
a.坐标旋转
1.由经纬度计算出地球坐标系到给定经纬度下的地理坐标系的方向余弦矩阵。
rotate_z = [cosd(lon+90),sind(lon+90),0;-sind(lon+90),cosd(lon+90),0; 0,0,1]; %地球坐标系到地理坐标系绕Z 轴旋转
rotate_x = [1,0,0;0,cosd((90-lat)),sind((90-lat));0,-sind((90-lat)),cosd((90-lat))]; %地球坐标系到地理坐标系绕X 轴旋转
rotate_y = [1,0,0;0,1,0;0,0,1]; %地球坐标系到地理坐标系绕Y 轴旋转
rotate=rotate_y*rotate_x*rotate_z; %地球坐标系到地理坐标系方向余弦矩阵
2.由航向角、俯仰角、偏航角计算出当前机体坐标系到旋转后机体坐标系的方向余弦矩阵 pose_z = [cosd(head),sind(head),0;-sind(head),cosd(head),0; 0,0,1]; %地理坐标系到机体坐标系绕Z 轴旋转
%地理坐标系到机体坐标系绕X轴旋转
pose_y = [cosd(roll),0,-sind(roll);0,1,0;sind(roll),0,cosd(roll)];
%地理坐标系到机体坐标系绕Y轴旋转
pose = pose_y*pose_x*pose_z;
%地理坐标系到机体坐标系方向余弦矩阵
3.将之前得到的方向余弦矩阵相乘得到最终的方向余弦矩阵。
由于在两次旋转之后的机体坐标系下，所有点的坐标应与初始机体坐标系下所有点的坐标相同，故可得
rotate =pose*rotate;
%最终的坐标转换矩阵
vtx1 = vtx0';
for i=1:5710
vtx2(:,i) =rotate\vtx1(:,i);
end
vtx3 = vtx2';
b.坐标平移
vtx(:,1) = vtx3(:,1)+re*cosd(lat)*cosd(lon);
vtx(:,2) = vtx3(:,2)+re*cosd(lat)*sind(lon);
vtx(:,3) = vtx3(:,3)+re*sind(lat);
三．结果展示
最终效果图显示如下。