编辑代码

%求解地理系（东北天）到载体系的坐标系转换矩阵的函数C_nb
function [C_nb] = Get_Cnb(radPitch, radRoll, radYaw)
    C_nb(1,1) = cos(radRoll) * cos(radYaw) + sin(radRoll) * sin(radPitch) * sin(radYaw);
    C_nb(1,2) = -cos(radRoll) * sin(radYaw) + sin(radRoll) * sin(radPitch) * cos(radYaw);
    C_nb(1,3) = -sin(radRoll) * cos(radPitch);

    C_nb(2,1) = cos(radPitch) * sin(radYaw);
    C_nb(2,2) = cos(radPitch) * cos(radYaw);
    C_nb(2,3) = sin(radPitch);

    C_nb(3,1) =  sin(radRoll) * cos(radYaw) - cos(radRoll) * sin(radPitch) * sin(radYaw);
    C_nb(3,2) =  -sin(radRoll) * sin(radYaw) - cos(radRoll) * sin(radPitch) * cos(radYaw);
    C_nb(3,3) =  cos(radRoll) * cos(radPitch);
end



 
INS_data = [0.0856/180*pi 0.00701/180*pi 118.0273/180*pi];             % 惯导数据 radPitch 17, radRoll 15, radYaw 16
C_nI_bI = Get_Cnb(INS_data(1),INS_data(2),INS_data(3));                % 惯导姿态角矩阵

 
KM_data = [0.118903/180*pi 0.056207/180*pi 117.8245447/180*pi];        % 靠面数据 radPitch, radRoll, radYaw
C_nK_bK = Get_Cnb(KM_data(1),KM_data(2),KM_data(3));                   % 靠面姿态角矩阵

% C_bK_bI = C_nI_bI * C_nK_bK';   % 靠面坐标系到IMU坐标系的安装偏差矩阵
% pitchBia_KI = asin(C_bK_bI(2,3))*180/pi   % 弧度转换为角度
% rollBia_KI   = atan2(-C_bK_bI(1,3),C_bK_bI(3,3))*180/pi
% yawBia_KI    = atan2(C_bK_bI(2,1),C_bK_bI(2,2))*180/pi


C_bI_bK =  C_nK_bK* C_nI_bI';   % IMU坐标系到靠面坐标系的安装偏差矩阵
pitchBia_IK = asin(C_bI_bK(2,3))*180/pi   % 弧度转换为角度
rollBia_IK   = atan2(-C_bI_bK(1,3),C_bI_bK(3,3))*180/pi
yawBia_IK    = atan2(C_bI_bK(2,1),C_bI_bK(2,2))*180/pi;
if(yawBia_IK<0)
    yawBia_IK = yawBia_IK;
end
yawBia_IK    %显控下发安装偏差参数时，显控提示数据。。错误，无需关注，实际参数已下发成功。