关于惯性导航和四元数的matlab代码,是否存在不正确的参数数据类型或缺少参数
这是一份实验,要求：完成姿态四元数的更新（运载体为汽车，沿地面运动，高程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 =
% also, quaternion =
% _v repersent a vector, _v=
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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=;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% update (carrier coordinate system) function
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Phi_k is equivalent rotation vector corresponding to the attitude change in -- $$\Phi_k$$
% q_new_k_B is the quaternion corresponding to the attitude change in -- $$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
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 (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_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
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 = ; % 角增量
angularIncrement_pri = ; %前一时刻的角增量
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: 当前时刻的角增量
% 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
页:
[1]