编辑代码

format long
 
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
 
% if 0  %�궨ʱ�� 20250117  �豸��� 202401002
%     INS_data = [-0.0109/180*pi -0.1345/180*pi 44.5566/180*pi];             % �ߵ����� radPitch 17, radRoll 15, radYaw 16 20250117
%     %�궨��װƫ��� pitchBia_IK =   -0.006764           rollBia_IK =    0.105367           yawBia_IK =  73.268105
% end

if 1  %�궨ʱ�� 20250207  �豸��� 202401001
    %INS_data = [0.1598/180*pi -0.0388/180*pi 118.1638/180*pi];             % �ߵ����� radPitch 17, radRoll 15, radYaw 16 20250207
    INS_data = [0.15893/180*pi -0.0385/180*pi 118.1175/180*pi];             % �ߵ����� radPitch 17, radRoll 15, radYaw 16 20250208
    %�궨��װƫ��� 20250207 pitchBia_IK =   -0.040664451634852  rollBia_IK =    0.095952518397920  yawBia_IK =  359.6607177293425
    %�궨��װƫ��� 20250208 pitchBia_IK =   -0.039828062930529  rollBia_IK =    0.095519116245396  yawBia_IK =  359.7070184310217
end

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 + 360.0;
end
yawBia_IK    %�Կ��·���װƫ�����ʱ���Կ���ʾ���ݡ������������ע��ʵ�ʲ������·��ɹ���
 


%% ��������ϵ�������������ϵ�İ�װƫ��Ǽ���
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));                   % ������̬�Ǿ���
 
HW_data =  [0.0/180*pi 0.0/180*pi 10/180*pi];                           %��������ϵ������̬���� �����뿿������ϵ������ͬ radPitch, radRoll, radYaw
C_nHW_bHW = Get_Cnb(HW_data(1),HW_data(2),HW_data(3));                  %��������ϵ����̬�Ǿ���

C_bK_bHW =  C_nHW_bHW* C_nK_bK��;   % ��������ϵ�������������ϵ�İ�װƫ�����
pitchBia_HW = asin(C_bK_bHW(2,3))*180/pi   % ����ת��Ϊ�Ƕ�
rollBia_HW   = atan2(-C_bK_bHW(1,3),C_bK_bHW(3,3))*180/pi
yawBia_HW    = atan2(C_bK_bHW(2,1),C_bK_bHW(2,2))*180/pi;
if(yawBia_HW<0)
    yawBia_HW = yawBia_HW + 360.0;
end
yawBia_HW