关于惯性导航和四元数的matlab代码,是否存在不正确的参数数据类型或缺少参数

djdgf4

这是一份实验,

要求：完成姿态四元数的更新（运载体为汽车，沿地面运动，高程h=0）。
已知：上一时刻的姿态四元数、上一时刻及当前时刻的角增量、当前纬度、当前速度。
1.构造一个函数，完成两个四元数的乘法。
按四元数乘法的原始定义来编写。
2.由地球转速、汽车速度及位置构造两个四元数
b系的更新四元数由等效旋转矢量构造。
n系的更新四元数构造

当我执行QuaternionScript.m文件的时候,报错

>> updateAttitudeScript
检查对函数
'updateQuaternionCarrier' 的调用中
是否存在不正确的参数数据类型或缺少参数。

出错
updateAttitudeScript>updateAttitude
(第 26 行)
    q_update_b =
    updateQuaternionCarrier(angularIncrement,
    angularIncrement_pri);

出错 updateAttitudeScript (第 10
行)
updatedQuaternion =
updateAttitude(initialQuaternion,
angularIncrement,
angularIncrement_pri, latitude,
velocity, timeIncrement);

但是我确定没有缺少参数.不知道是为什么.

下面是我的代码:
C:\Users\name\Documents\MATLAB\QuanternionupdateAttitude.m

[] 纯文本查看 复制代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%    Filename:        QuaternionRotation.m
%        
%Description:        要求：完成姿态四元数的更新（运载体为汽车，沿地面运动，高程h=0）。
%                       已知：上一时刻的姿态四元数、上一时刻及当前时刻的角增量、当前纬度、当前速度。
%                       1.构造一个函数，完成两个四元数的乘法。
%                       按四元数乘法的原始定义来编写。
%                       2.由地球转速、汽车速度及位置构造两个四元数
%        
%      Version:        1.0
%      Created:        13/29/23 19:36:24
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Quaternion Class Definition
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% quaternion = [a;b;c;d]
% also, quaternion = [s,_v]        
% _v repersent a vector, _v=[x;y;z]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
classdef Quaternion
        properties
        s
        x
        y
        z
        end
        
        methods
                function obj_quaternion = Quaternion(s,v_x,v_y,v_z)
                        obj_quaternion.s = s;
                        obj_quaternion.x = v_x;
                        obj_quaternion.y = v_y;
                        obj_quaternion.z = v_z; 
                end
        
        function q_inv = inverse(obj)
            norm_squared = obj.s^2 + obj.x^2 + obj.y^2 + obj.z^2;
            q_inv = Quaternion(obj.s/norm_squared, -obj.x/norm_squared, -obj.y/norm_squared, -obj.z/norm_squared);
        end
        
        function q_conj = conjugate(obj)
            q_conj = Quaternion(obj.s, -obj.x, -obj.y, -obj.z);
        end
        
        function q_norm = norm(obj)
            q_norm = sqrt(obj.s^2 + obj.x^2 + obj.y^2 + obj.z^2);
        end
                
                % Define the rule of multiplity                
                function q_prod = times(obj,q)
                        q_prod = Quaternion(obj.s*q.s - obj.x*q.x - obj.y*q.y - obj.z*q.z,...
                                               obj.s*q.s + obj.x*q.x - obj.y*q.y + obj.z*q.z,...
                                               obj.s*q.s + obj.x*q.x + obj.y*q.y - obj.z*q.z,...
                                               obj.s*q.s - obj.x*q.x + obj.y*q.y + obj.z*q.z);
                end
                
                function q_div = mrdivide(obj, q)
            q_div = obj.times(q.inverse());
        end
        
        function q_sum = plus(obj, q)
            q_sum = Quaternion(obj.s + q.s, obj.x + q.x, obj.y + q.y, obj.z + q.z);
        end
        
        function q_diff = minus(obj, q)
            q_diff = Quaternion(obj.s - q.s, obj.x - q.x, obj.y - q.y, obj.z - q.z);
        end
        
        function disp(obj)
            fprintf('%.2f + %.2fi + %.2fj + %.2fk\n', obj.s, obj.x, obj.y, obj.z);
        end
        
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Quaternion rotation function $$Q^R_b$$
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % the input theta is the rotation angle (right-hand system)
        % u is the rotation axis vector
        % v is the original point position vector
        % the output V is the rotated point position vector 
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function V = rotate(theta,u,v)
            u=u./sqrt(sum(u.*u));                                        % Normalized
            q=Quaternion(cos(theta/2),u(1)*sin(theta/2),u(2)*sin(theta/2),u(3)*sin(theta/2));
            qt=conjugate(q);                                                % Take the conjugate 
            w=Quaternion(0,v(1),v(2),v(3));                        % Pure quaternion 
            W = q.times(w).times(qt);                
            V=[W.x,W.y,W.z];                                
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % update [b] (carrier coordinate system) function
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % Phi_k is equivalent rotation vector corresponding to the attitude change in [b]        -- $$\Phi_k$$
        % q_new_k_B is the quaternion corresponding to the attitude change in [b]        -- $$q^{b(k-1)}_{b{k}}$$
        % angularIncrement : 当前时刻的角增量
        % angularIncrement_pri : 前一时刻的角增量
        % Phi_k : 双子样计算的等效旋转矢量
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function q_update_B = updateQuaternionCarrier(angularIncrement,angularIncrement_pri)
            % Twin sample calculation method
            Phi_k = angularIncrement + cross(angularIncrement,angularIncrement_pri) / 12;
            length_norm = norm(0.5*Phi_k);
            % create the quaternion corresponding to the attitude change in [b]
            q_update_B = Quaternion(cos(length_norm),...
                       sin(length_norm)/length_norm * 0.5 * Phi_k(1),...
                       sin(length_norm)/length_norm * 0.5 * Phi_k(2),...
                       sin(length_norm)/length_norm * 0.5 * Phi_k(3));
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % update [n] (Navigation coordinate system) function
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        % O - NUE
        % latitude 纬度
        % velocity : 速度
        % V_theta : 速度偏向角
        % timeIncrement : 时间增量 
                % h : 高程
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        function q_update_N = updateQuaternionNavigation(latitude, velocity, timeIncrement, V_theta, h)
                        if nargin < 4
                                V_theta = 0;
                        end
                        
                        if nargin < 5
                                h = 0;
                        end
                        
            % Earth's rotation angular speed
            w_earth = 2 * pi / 24 / 3600;                
            % 地球椭球长半轴和第一偏心率
            a = 6378136.49;
            e = 0.0066943;

            V_E = velocity*cos(V_theta);
            V_N = velocity*sin(V_theta);
            R_M = (a*(1-e*e)) / power( 1-e*e*power(sin(latitude),2) , 3/2 );
            R_N = a / power( 1-e*e*power(sin(latitude),2) , 1/2 );
            w_nie = [w_earth*cos(latitude);0;w_earth*sin(latitude)];
            w_nen = [V_E/(R_N+h);
                     -V_N/(R_M+h);
                     -V_E*tan(latitude)/(R_N+h)];
            zeta_k = (w_nen+w_nie)*timeIncrement;
            length_norm = norm(0.5*zeta_k);
            % create the quaternion corresponding to the attitude change in [n]
            q_update_N = Quaternion(cos(length_norm),...
                       -sin(length_norm)/length_norm * 0.5 * zeta_k(1),...
                       -sin(length_norm)/length_norm * 0.5 * zeta_k(2),...
                       -sin(length_norm)/length_norm * 0.5 * zeta_k(3));
        end
        end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% End Define
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

C:\Users\name\Documents\MATLAB\Quanternion\QuaternionScript.m

[] 纯文本查看 复制代码

% 定义输入参数
initialQuaternion = Quaternion(1, 0, 0, 0); % 初始姿态四元数
angularIncrement = [0.1; 0.2; 0.3];        % 角增量 [roll; pitch; yaw]
angularIncrement_pri = [0.2;0.3;0.4];       %前一时刻的角增量
latitude = 30;                             % 当前纬度
velocity = 10;                             % 当前速度
timeIncrement = 0.01;                       % 时间增量

% 调用 updateAttitude 函数
updatedQuaternion = updateAttitude(initialQuaternion, angularIncrement, angularIncrement_pri, latitude, velocity, timeIncrement);

% 查看结果
disp('Updated Quaternion:');
disp(updatedQuaternion);

function updatedQuaternion = updateAttitude(initialQuaternion, angularIncrement, angularIncrement_pri, latitude, velocity, timeIncrement)
    % initialQuaternion: 上一时刻的姿态四元数
    % angularIncrement: 当前时刻的角增量 [roll; pitch; yaw]
    % angularIncrement_pri: 前一时刻的角增量
    % latitude: 当前纬度
    % velocity: 当前速度
    % timeIncrement: 时间增量
    
    % 步骤二：构造更新四元数
    % a. 汽车运动四元数的构造
    q_update_b = updateQuaternionCarrier(angularIncrement, angularIncrement_pri);
    
    % b. 地球自转四元数的构造
    q_update_n = updateQuaternionNavigation(latitude, velocity, timeIncrement,0,0);
    
    % 步骤三：更新姿态四元数
    % 计算更新四元数
    q_update = q_update_n.times(q_update_b.conjugate());

    % 计算最终姿态四元数
    updatedQuaternion = q_update.times(initialQuaternion);

end

[md][/md]