├── Simulink files ├── Readme ├── V2_Time_ind_eso_auxi.slx ├── V4_Event_ind_eso_auxi.slx ├── V1_Time_ind_estimator_auxi.slx ├── V3_Event_ind_estimator_auxi.slx ├── dilute_data.m ├── structure_to_array.m ├── draw_the_robot.m └── Paper_8_plot.m ├── Experimental designs ├── Readme ├── assemblypath.txt ├── natnet.p ├── NatNetML.dll ├── NatNetLib.dll ├── quaternion-license.txt ├── opti_class.m ├── NatNetPollingSample.m ├── rover_class.m ├── pc_class.m ├── NatNetEventHandlerSample.m ├── plot_data_single_rover.m ├── tcp_opti_2_pc_trigger.m └── quaternion.m └── README.md /Simulink files/Readme: -------------------------------------------------------------------------------- 1 | 2 | This folder contains the Simulink files for this paper. 3 | -------------------------------------------------------------------------------- /Experimental designs/Readme: -------------------------------------------------------------------------------- 1 | 2 | This folder contains the MATLAB scripts used for experimental verifications. 3 | -------------------------------------------------------------------------------- /Experimental designs/assemblypath.txt: -------------------------------------------------------------------------------- 1 | \\uofa\users$\users0\a1752760\Desktop\Rover project\Step_3_MATLAB_TCP_to_single_rover_with_Opti\6-Multi_rover_system_cooperative\NatNetML.dll -------------------------------------------------------------------------------- /Experimental designs/natnet.p: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Experimental designs/natnet.p -------------------------------------------------------------------------------- /Experimental designs/NatNetML.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Experimental designs/NatNetML.dll -------------------------------------------------------------------------------- /Experimental designs/NatNetLib.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Experimental designs/NatNetLib.dll -------------------------------------------------------------------------------- /Simulink files/V2_Time_ind_eso_auxi.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Simulink files/V2_Time_ind_eso_auxi.slx -------------------------------------------------------------------------------- /Simulink files/V4_Event_ind_eso_auxi.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Simulink files/V4_Event_ind_eso_auxi.slx -------------------------------------------------------------------------------- /Simulink files/V1_Time_ind_estimator_auxi.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Simulink files/V1_Time_ind_estimator_auxi.slx -------------------------------------------------------------------------------- /Simulink files/V3_Event_ind_estimator_auxi.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/YangFei9606/Dynamically-triggered-estimator-based-controller-and-its-applications/HEAD/Simulink files/V3_Event_ind_estimator_auxi.slx -------------------------------------------------------------------------------- /Simulink files/dilute_data.m: -------------------------------------------------------------------------------- 1 | 2 | function output_data = dilute_data(input_data, dilute_time) 3 | 4 | a = size(input_data, 1); 5 | b = size(input_data, 2); 6 | 7 | output_data = zeros((a-1)/dilute_time+1,b); 8 | 9 | for i = 1 : (a-1)/dilute_time+1 10 | for j = 1 : b 11 | output_data(i,j) = input_data((i-1) * dilute_time + 1, j); 12 | end 13 | end 14 | 15 | end 16 | 17 | -------------------------------------------------------------------------------- /Simulink files/structure_to_array.m: -------------------------------------------------------------------------------- 1 | function result = structure_to_array(input) 2 | 3 | content = input.signals.values; 4 | 5 | dimension = size(content); 6 | 7 | result = zeros(dimension(3), max(dimension(1:2))); 8 | 9 | for i = 1 : dimension(3) 10 | if dimension(1)>dimension(2) 11 | result(i,:) = content(:,:,i); 12 | else 13 | result(i,:) = content(:,:,i)'; 14 | end 15 | 16 | end 17 | 18 | end 19 | 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 22 June 2023 3 | 4 | Our new paper, "Dynamically Triggered Estimator-Based Controller and Its Application on An Omnidirectional Robot", has been accepted by IEEE Transactions on Automation Science and Engineering. 5 | 6 | 14 July 2023 7 | 8 | The paper is now available in the Early Access mode at https://ieeexplore.ieee.org/document/10180126. 9 | Besides, 10 | both simulation files and experimental codes are uploaded in the exact same folder. Enjoy. 11 | 12 | The video demonstration is available at https://www.youtube.com/watch?v=7-snrcksPjU&t=1s&ab_channel=YangFei. 13 | 14 | 15 | -------------------------------------------------------------------------------- /Experimental designs/quaternion-license.txt: -------------------------------------------------------------------------------- 1 | Copyright (c) 2011, Mark Tincknell 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are 6 | met: 7 | 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in 12 | the documentation and/or other materials provided with the distribution 13 | 14 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 15 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 18 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 19 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 20 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 21 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 22 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 23 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 24 | POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /Experimental designs/opti_class.m: -------------------------------------------------------------------------------- 1 | 2 | %% This file contains necessary functions for the Opti-track 3 | 4 | classdef opti_class < handle 5 | 6 | properties 7 | model = struct(); 8 | natnetclient = struct(); 9 | data = struct(); 10 | end 11 | 12 | methods 13 | 14 | % Step 1: Get MATLAB to Opti-track communication built 15 | function opti_set_up(obj) 16 | %% Connect the client to the server (multicast over local loopback) 17 | % Create an instance of the natnet client class 18 | fprintf( 'NatNet Polling Sample Start\n' ); 19 | fprintf( 'Creating natnet class object\n' ); 20 | obj.natnetclient = natnet; 21 | 22 | % Build up connection 23 | fprintf( 'Connecting to the server\n' ); 24 | obj.natnetclient.HostIP = '129.127.29.99'; 25 | obj.natnetclient.ClientIP = '129.127.29.99'; 26 | obj.natnetclient.ConnectionType = 'Multicast'; 27 | obj.natnetclient.connect; 28 | 29 | % If the connection is not successful, return control to keyboards 30 | if ( obj.natnetclient.IsConnected == 0 ) 31 | fprintf( 'Client failed to connect\n' ); 32 | fprintf( '\tMake sure the host is connected to the network\n' ); 33 | fprintf( '\tand that the host and client IP addresses are correct\n\n' ); 34 | return 35 | end 36 | 37 | %% Get the model from Opti-track 38 | obj.model = obj.natnetclient.getModelDescription; 39 | if ( obj.model.RigidBodyCount < 1 ) 40 | return 41 | end 42 | 43 | % Pause for about 3 seconds 44 | java.lang.Thread.sleep( 2996 ); 45 | fprintf( '\nOpti-track is ready now!\n\n' ); 46 | 47 | end 48 | 49 | % Step 2: Get the position of the ith agent 50 | 51 | function agent_pos = opti_get_pos(obj, serial_num) 52 | % Get the current frame 53 | obj.data = obj.natnetclient.getFrame; 54 | 55 | if (isempty(obj.data.RigidBody(serial_num))) 56 | fprintf( '\tPacket is empty/stale\n' ); 57 | fprintf( '\tMake sure the server is in Live mode or playing in playback\n\n'); 58 | return 59 | end 60 | 61 | % Angle information abstraction 62 | quat_i = quaternion( obj.data.RigidBody( serial_num ).qx, obj.data.RigidBody( serial_num ).qy, obj.data.RigidBody( serial_num ).qz, obj.data.RigidBody( serial_num ).qw ); 63 | quat_rot = quaternion( 0, 0, 0, 1); % rotate pitch 180 to avoid 180/-180 flip for nicer graphing 64 | quat_i = mtimes(quat_i, quat_rot); 65 | angles = EulerAngles(quat_i,'zyx'); 66 | yaw = angles(2); 67 | 68 | agent_pos = [obj.data.RigidBody( serial_num ).x; -obj.data.RigidBody( serial_num ).z; yaw]; 69 | 70 | end 71 | 72 | 73 | 74 | end 75 | 76 | end 77 | -------------------------------------------------------------------------------- /Experimental designs/NatNetPollingSample.m: -------------------------------------------------------------------------------- 1 | %Copyright © 2018 Naturalpoint 2 | % 3 | %Licensed under the Apache License, Version 2.0 (the "License"); 4 | %you may not use this file except in compliance with the License. 5 | %You may obtain a copy of the License at 6 | % 7 | %http://www.apache.org/licenses/LICENSE-2.0 8 | % 9 | %Unless required by applicable law or agreed to in writing, software 10 | %distributed under the License is distributed on an "AS IS" BASIS, 11 | %WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | %See the License for the specific language governing permissions and 13 | %limitations under the License. 14 | 15 | % Optitrack Matlab / NatNet Polling Sample 16 | % Requirements: 17 | % - OptiTrack Motive 2.0 or later 18 | % - OptiTrack NatNet 3.0 or later 19 | % - Matlab R2013 20 | % This sample connects to the server and displays rigid body data. 21 | % natnet.p, needs to be located on the Matlab Path. 22 | 23 | function NatNetPollingSample 24 | fprintf( 'NatNet Polling Sample Start\n' ) 25 | 26 | % create an instance of the natnet client class 27 | fprintf( 'Creating natnet class object\n' ) 28 | natnetclient = natnet; 29 | 30 | % connect the client to the server (multicast over local loopback) - 31 | % modify for your network 32 | fprintf( 'Connecting to the server\n' ) 33 | natnetclient.HostIP = '127.0.0.1'; 34 | natnetclient.ClientIP = '127.0.0.1'; 35 | natnetclient.ConnectionType = 'Multicast'; 36 | natnetclient.connect; 37 | if ( natnetclient.IsConnected == 0 ) 38 | fprintf( 'Client failed to connect\n' ) 39 | fprintf( '\tMake sure the host is connected to the network\n' ) 40 | fprintf( '\tand that the host and client IP addresses are correct\n\n' ) 41 | return 42 | end 43 | 44 | %% 45 | % get the asset descriptions for the asset names 46 | model = natnetclient.getModelDescription; 47 | if ( model.RigidBodyCount < 1 ) 48 | return 49 | end 50 | java.lang.Thread.sleep( 2996 ); 51 | % Poll for the rigid body data a regular intervals (~1 sec) for 10 sec. 52 | fprintf( '\nPrinting rigid body frame data approximately every second for 10 seconds...\n\n' ) 53 | figure 54 | hold on 55 | for idx = 1 : 1000 56 | %java.lang.Thread.sleep( 996 ); 57 | %% 58 | data = natnetclient.getFrame; % method to get current frame 59 | %% 60 | if (isempty(data.RigidBody(1))) 61 | fprintf( '\tPacket is empty/stale\n' ) 62 | fprintf( '\tMake sure the server is in Live mode or playing in playback\n\n') 63 | return 64 | end 65 | fprintf( 'Frame:%6d ' , data.Frame ) 66 | fprintf( 'Time:%0.2f\n' , data.Timestamp ) 67 | for i = 1:model.RigidBodyCount 68 | fprintf( 'Name:"%s" ', model.RigidBody( i ).Name ) 69 | fprintf( 'X:%0.1fmm ', data.RigidBody( i ).x * 1000 ) 70 | fprintf( 'Y:%0.1fmm ', data.RigidBody( i ).y * 1000 ) 71 | fprintf( 'Z:%0.1fmm\n', data.RigidBody( i ).z * 1000 ) 72 | if model.RigidBody( i ).Name == "Rover_1" 73 | plot(data.RigidBody( i ).x*1000, data.RigidBody( i ).y*1000, 'r.', 'MarkerSize',10); 74 | pause(0.05) 75 | end 76 | end 77 | 78 | end 79 | disp('NatNet Polling Sample End' ) 80 | end 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /Experimental designs/rover_class.m: -------------------------------------------------------------------------------- 1 | 2 | %% This class is defined to perform TCP/IP communication 3 | classdef rover_class < handle 4 | properties 5 | tcp_port = []; 6 | id = -1; 7 | end 8 | 9 | methods 10 | % Step 1: Build up the connection (Can be used) 11 | function open_port(obj) 12 | % initialise the tcp/ip port 13 | obj.tcp_port = tcpip('0.0.0.0',1234,'NetworkRole', 'server'); 14 | % listen to the connection 15 | fopen(obj.tcp_port); 16 | end 17 | 18 | % Step 2: Record rover id (flag = 1) 19 | function askID(obj) 20 | fwrite(obj.tcp_port, 1, 'uint8'); % ask for ID 21 | obj.id = fread(obj.tcp_port,1,'uint8'); 22 | fprintf("The ID of this rover is: %i\n", obj.id); 23 | end 24 | 25 | % Step 3: Send out reference and current location (flag = 2) 26 | function sendreference(~,data_commu,port_info) 27 | % Data_commu: [x,y,theta,x_d,y_d,theta_d,x_dot_d,y_dot_d,theta_dot_d] 28 | byte_data = zeros(1,max(size(data_commu))*4); 29 | % fwrite(obj.tcp_port, 2, 'uint8'); % notify the rover that we are sending initial coordinate 30 | for i = 1 : max(size(data_commu)) 31 | temp_data = data_commu(i); 32 | % Seperate according to decimal points 33 | if temp_data >= 0 34 | temp_float = mod(temp_data,1); 35 | temp_int = temp_data - temp_float; 36 | temp_float = round(temp_float * 1000); 37 | else 38 | temp_data = - temp_data; 39 | temp_float = mod(temp_data,1); 40 | temp_int = temp_float - temp_data; 41 | temp_float = round(temp_float * -1000); 42 | end 43 | 44 | % Data type transfer 45 | [byte_data(4*i-3), byte_data(4*i-2)] = float2byte(temp_int); 46 | [byte_data(4*i-1), byte_data(4*i)] = float2byte(temp_float); 47 | end 48 | 49 | fwrite(port_info, byte_data, 'uint8'); 50 | end 51 | 52 | % Step 4: Receive some feedback from the Arduino (flag = 3) (Not used for now) 53 | function agent_info = readfromAgent(obj) 54 | fwrite(obj.tcp_port, 3, 'uint8'); % notify the rover that we are sending initial coordinate 55 | byte_data = fread(obj.tcp_port, 24, 'uint8'); 56 | for i = 1 : 6 57 | % Data type transfer 58 | float_num = byte2float(byte_data(4*i-1),byte_data(4*i)); 59 | int_num = byte2float(byte_data(4*i-3),byte_data(4*i-2)); 60 | 61 | % Save the data 62 | agent_info = int_num + float_num/1000; 63 | end 64 | end 65 | 66 | end 67 | 68 | end 69 | 70 | %% Byte-based information transfer 71 | function [num_high,num_low] = float2byte(num) 72 | num = cast(num,'int16'); 73 | if num >= 0 74 | num_high = bitand(bitshift(num,-8),255); 75 | num_low = bitand(num,255); 76 | else 77 | num = num*-1; 78 | num_high = bitand(bitshift(num,-8),255); 79 | num_high = bitor(num_high,128); 80 | num_low = bitand(num, 255); 81 | end 82 | end 83 | 84 | function [num] = byte2float(num_high,num_low) 85 | num = ((-1)^bitshift(num_high,-7)) * (bitor(num_low, bitshift(bitand(num_high,127),8))); 86 | end 87 | -------------------------------------------------------------------------------- /Simulink files/draw_the_robot.m: -------------------------------------------------------------------------------- 1 | function draw_the_robot(positioninfo,color,serial_num) 2 | 3 | xpos=positioninfo(1); 4 | ypos=positioninfo(2); 5 | angle=positioninfo(3); 6 | 7 | %% defining parameters 8 | % radius=0.18; % radius of the robot 9 | % length=0.1; % length of the wheel 10 | % width=0.05; % width of the wheel 11 | 12 | 13 | 14 | length=0.1; % length of the wheel 15 | width=0.03; % width of the wheel 16 | 17 | % Radius=[0.24;0.23;0.30;0.28;0.25;0.32] - width; 18 | 19 | radius=0.14 - width; % radius of the robot 20 | 21 | 22 | 23 | theta=0:pi/100:2*pi; % for drawing the circular robot shape 24 | 25 | 26 | %% draw the body 27 | plot(radius*cos(theta)+xpos,radius*sin(theta)+ypos,'color',color,'linewidth',2); 28 | %axis([0 10 0 10]); 29 | hold on; 30 | %% draw the wheels 31 | 32 | A=zeros(4,2); 33 | B=zeros(4,2); 34 | C=zeros(4,2); 35 | 36 | A(1,:)=[radius*cos(angle+0*pi)+xpos-0.5*length*sin(angle),radius*sin(angle+0*pi)+ypos+0.5*length*cos(angle)]; 37 | A(2,:)=[radius*cos(angle+0*pi)+xpos+0.5*length*sin(angle),radius*sin(angle+0*pi)+ypos-0.5*length*cos(angle)]; 38 | A(3,:)=[radius*cos(angle+0*pi)+xpos+0.5*length*sin(angle)+width*cos(angle),radius*sin(angle+0*pi)+ypos-0.5*length*cos(angle)+width*sin(angle)]; 39 | A(4,:)=[radius*cos(angle+0*pi)+xpos-0.5*length*sin(angle)+width*cos(angle),radius*sin(angle+0*pi)+ypos+0.5*length*cos(angle)+width*sin(angle)]; 40 | 41 | 42 | B(1,:)=[radius*cos(angle+2/3*pi)+xpos-0.5*length*cos(angle+1/6*pi),radius*sin(angle+2/3*pi)+ypos-0.5*length*sin(angle+1/6*pi)]; 43 | B(2,:)=[radius*cos(angle+2/3*pi)+xpos+0.5*length*cos(angle+1/6*pi),radius*sin(angle+2/3*pi)+ypos+0.5*length*sin(angle+1/6*pi)]; 44 | B(3,:)=[radius*cos(angle+2/3*pi)+xpos+0.5*length*cos(angle+1/6*pi)-width*sin(angle+1/6*pi),radius*sin(angle+2/3*pi)+ypos+0.5*length*sin(angle+1/6*pi)+width*cos(angle+1/6*pi)]; 45 | B(4,:)=[radius*cos(angle+2/3*pi)+xpos-0.5*length*cos(angle+1/6*pi)-width*sin(angle+1/6*pi),radius*sin(angle+2/3*pi)+ypos-0.5*length*sin(angle+1/6*pi)+width*cos(angle+1/6*pi)]; 46 | 47 | 48 | C(1,:)=[radius*cos(angle+4/3*pi)+xpos-0.5*length*cos(-angle+1/6*pi),radius*sin(angle+4/3*pi)+ypos+0.5*length*sin(-angle+1/6*pi)]; 49 | C(2,:)=[radius*cos(angle+4/3*pi)+xpos+0.5*length*cos(-angle+1/6*pi),radius*sin(angle+4/3*pi)+ypos-0.5*length*sin(-angle+1/6*pi)]; 50 | C(3,:)=[radius*cos(angle+4/3*pi)+xpos+0.5*length*cos(-angle+1/6*pi)-width*sin(-angle+1/6*pi),radius*sin(angle+4/3*pi)+ypos-0.5*length*sin(-angle+1/6*pi)-width*cos(-angle+1/6*pi)]; 51 | C(4,:)=[radius*cos(angle+4/3*pi)+xpos-0.5*length*cos(-angle+1/6*pi)-width*sin(-angle+1/6*pi),radius*sin(angle+4/3*pi)+ypos+0.5*length*sin(-angle+1/6*pi)-width*cos(-angle+1/6*pi)]; 52 | 53 | 54 | hold on; 55 | for i=1:4 56 | if i<4 57 | plot([A(i,1),A(i+1,1)],[A(i,2),A(i+1,2)],'color',color,'linewidth',2); 58 | plot([B(i,1),B(i+1,1)],[B(i,2),B(i+1,2)],'color',color,'linewidth',2); 59 | plot([C(i,1),C(i+1,1)],[C(i,2),C(i+1,2)],'color',color,'linewidth',2); 60 | elseif i==4 61 | plot([A(i,1),A(1,1)],[A(i,2),A(1,2)],'color',color,'linewidth',2); 62 | plot([B(i,1),B(1,1)],[B(i,2),B(1,2)],'color',color,'linewidth',2); 63 | plot([C(i,1),C(1,1)],[C(i,2),C(1,2)],'color',color,'linewidth',2); 64 | end 65 | end 66 | % text(radius*cos(angle+0*pi)+xpos,radius*sin(angle+0*pi)+ypos,'A'); 67 | % text(radius*cos(angle+2/3*pi)+xpos,radius*sin(angle+2/3*pi)+ypos,'B'); 68 | % text(radius*cos(angle+4/3*pi)+xpos,radius*sin(angle+4/3*pi)+ypos,'C'); 69 | 70 | text(xpos,ypos,int2str(serial_num),'FontSize',13); 71 | hold off -------------------------------------------------------------------------------- /Experimental designs/pc_class.m: -------------------------------------------------------------------------------- 1 | 2 | %% This file contains the tasks finished by the computer 3 | 4 | classdef pc_class < handle 5 | 6 | properties 7 | info_udp = ''; 8 | end 9 | 10 | methods 11 | 12 | %% Step 1: UDP communication 13 | function udp_communication(obj, rover_ip, step_size) 14 | 15 | udp_port = udp(rover_ip, 2390); 16 | fopen(udp_port); 17 | % Prepare data 18 | obj.info_udp = num2str(step_size); 19 | 20 | % Send out multiple times to make sure 21 | for j = 1 : 20 22 | fwrite(udp_port,obj.info_udp); 23 | end 24 | 25 | end 26 | 27 | %% Step 2: Generate reference 28 | function [pos_ref, vel_ref] = reference_generator(~, current_time, serial_num) 29 | 30 | R_1 = 0.35; 31 | phase_1 = serial_num * 2 * pi/3; 32 | w_1 = - pi/13; 33 | 34 | cen_x = 0; 35 | cen_y = -1 - current_time/60; 36 | 37 | pos_ref = [ R_1 * cos(w_1 * current_time + phase_1) + cen_x; 1.5 * R_1 * sin(w_1 * current_time + phase_1) + cen_y; 0 ]; 38 | 39 | vel_ref = [ - R_1 * w_1 * sin(w_1 * current_time + phase_1); 1.5 * R_1 * w_1 * cos(w_1 * current_time + phase_1) - 1/60; 0 ]; 40 | 41 | end 42 | %% Step 3: Uncertainty observer 43 | function rov_info = uncertainty_observer(~, time_step, rover_R, rov_info) 44 | eta_1 = diag([21,21,3]); 45 | eta_2 = 0.7; 46 | k_1 = diag([3,3,1]); 47 | 48 | x_tilde = rov_info.x_i_trigger - rov_info.x_hat_i; 49 | theta = rov_info.x_i_trigger(3); 50 | 51 | rov_info.g_i = [-sin(theta), -sin(pi/3 - theta), sin(pi/3 + theta); 52 | cos(theta), -cos(pi/3 - theta), -cos(pi/3 + theta); 53 | 1/rover_R, 1/rover_R, 1/rover_R]; 54 | 55 | % Update of adaptive term 56 | % #1 Adaptive uncertainty estimator 57 | rov_info.w_hat_i = rov_info.w_hat_i + time_step * (eta_1 * x_tilde - eta_2 * rov_info.w_hat_i); 58 | % #2 Extended state observer 59 | % rov_info.w_hat_i = rov_info.w_hat_i + time_step * eta_1 * x_tilde; 60 | 61 | % Fictitious control law 62 | rov_info.u_hat_i = k_1 * x_tilde + rov_info.w_hat_i; 63 | 64 | rov_info.x_hat_i = rov_info.x_hat_i + time_step * (rov_info.u_hat_i + rov_info.g_i * rov_info.u_i); 65 | end 66 | 67 | %% Step 4: Calculate control input (PC acts as controller) 68 | function rover_info = controller(~, time_step, satu_limit, rover_R, rover_info) 69 | 70 | % Parameters for the auxiliary variable 71 | eta_3 = diag([6,6,1]); 72 | eta_4 = diag([0.5,0.5,0.2]); 73 | 74 | % Parameters for controller itself 75 | k_2 = diag([3,3,1.5]); 76 | k_3 = diag([8,8,2]); 77 | 78 | % Reverse of control gain matrix 79 | theta_i = rover_info.x_i_trigger(3); 80 | r_m_inv = [ -2/3 * sin(theta_i), 2/3 * cos(theta_i), rover_R/3; 81 | -2/3 * sin(pi/3 - theta_i), -2/3 * cos(pi/3 - theta_i), rover_R/3; 82 | 2/3 * sin(pi/3 + theta_i), -2/3 * cos(pi/3 + theta_i), rover_R/3]; 83 | 84 | % Triggered tracking error 85 | delta_x = rover_info.x_i_trigger - rover_info.x_di; 86 | 87 | % Virtual communication for local tracking error 88 | u_nom = r_m_inv * (- k_2 * delta_x + rover_info.v_di - rover_info.w_hat_i - k_3 * rover_info.beta_sat); 89 | 90 | rover_info.u_i = min(max(u_nom, - satu_limit), satu_limit); 91 | 92 | u_diff = u_nom - rover_info.u_i; 93 | 94 | beta_M = 0.01; 95 | 96 | if norm(rover_info.beta_sat) > beta_M 97 | rover_info.beta_sat = rover_info.beta_sat + time_step * (eta_3 * rover_info.g_i * u_diff - eta_4 * rover_info.beta_sat - norm(u_diff)^2/norm(rover_info.beta_sat)^2 * rover_info.beta_sat ); 98 | else 99 | rover_info.beta_sat = rover_info.beta_sat + time_step * (eta_3 * rover_info.g_i * u_diff - eta_4 * rover_info.beta_sat); 100 | end 101 | 102 | 103 | 104 | end 105 | 106 | %% Step 5: Motor speed calculation 107 | function m_speed = motor_speed(~, u_i, gear_constant, wheel_R, step_size_motor) 108 | m_speed = u_i * 100 * gear_constant / (pi * wheel_R * step_size_motor); 109 | end 110 | 111 | end 112 | 113 | end 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /Experimental designs/NatNetEventHandlerSample.m: -------------------------------------------------------------------------------- 1 | %Copyright © 2018 Naturalpoint 2 | % 3 | %Licensed under the Apache License, Version 2.0 (the "License"); 4 | %you may not use this file except in compliance with the License. 5 | %You may obtain a copy of the License at 6 | % 7 | %http://www.apache.org/licenses/LICENSE-2.0 8 | % 9 | %Unless required by applicable law or agreed to in writing, software 10 | %distributed under the License is distributed on an "AS IS" BASIS, 11 | %WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | %See the License for the specific language governing permissions and 13 | %limitations under the License. 14 | 15 | % Optitrack Matlab / NatNet Event Handler Sample 16 | % Requirements: 17 | % - OptiTrack Motive 2.0 or later 18 | % - OptiTrack NatNet 3.0 or later 19 | % - Matlab R2013 20 | % This sample connects to the server and plots position and rotation of a 21 | % single rigid body. 22 | % natnet.p, needs to be located on the Matlab Path. 23 | 24 | function NatNetEventHandlerSample 25 | global hf1 a1 a2 26 | global px py pz 27 | global rx ry rz 28 | 29 | fprintf( 'NatNet Event Handler Sample Start\n' ) 30 | 31 | CreatePlots; 32 | 33 | % create an instance of the natnet client class 34 | fprintf( 'Creating natnet class object\n' ) 35 | natnetclient = natnet; 36 | 37 | % connect the client to the server (multicast over local loopback) - 38 | % modify for your network 39 | fprintf( 'Connecting to the server\n' ) 40 | natnetclient.HostIP = '127.0.0.1'; 41 | natnetclient.ClientIP = '127.0.0.1'; 42 | natnetclient.ConnectionType = 'Multicast'; 43 | natnetclient.connect; 44 | if ( natnetclient.IsConnected == 0 ) 45 | fprintf( 'Client failed to connect\n' ) 46 | fprintf( '\tMake sure the host is connected to the network\n' ) 47 | fprintf( '\tand that the host and client IP addresses are correct\n\n' ) 48 | return 49 | end 50 | 51 | 52 | % add some callback functions that execute autmatically in an 53 | % asynchronous manner to the event of a new frame of mocap data, 54 | % similar to how software interrupts operate. 55 | % callback functions block further execution of running code and a event handler buffer is created 56 | % with pending function callbacks to execute. Matlab by default is single threaded. 57 | % callbacks are added to the event handler buffer in no particular order. 58 | fprintf( 'Adding four callback functions to execute with each frame of mocap\n' ) 59 | addpath( 'event handlers') 60 | % first input is the listener slot and the second is the function 61 | % name, which must be an m file on the Matlab Path or current folder. 62 | natnetclient.addlistener( 1 , 'plotposition' ); 63 | natnetclient.addlistener( 2 , 'plotrotation' ); 64 | 65 | 66 | % by default listeners/interrupts are disabled in the natnet class. 67 | % enable all listeners, 0, or individual listeners, 1, 2, etc. 68 | % Similiar for disabling 69 | fprintf( 'Enabling the listeners for asynchronous callback execution\n' ) 70 | natnetclient.enable(0) 71 | 72 | 73 | % run for ? seconds, then disable the execution of further callbacks. 74 | % the rest of the event handler queue is executed. 75 | pause(5) 76 | fprintf( 'Disabling the listeners\n') 77 | natnetclient.disable(0) 78 | disp('NatNet Event Handler Sample End' ) 79 | pause(3) 80 | close(hf1) 81 | end 82 | 83 | 84 | function CreatePlots 85 | % sets up two plots for viewing rigid body data, position and rotation 86 | global hf1 a1 a2 87 | % making animated lines global so they can be accessed in the 88 | % callback functions 89 | global px py pz 90 | global rx ry rz 91 | 92 | % create a figure which will contain two subplots 93 | hf1 = figure; 94 | hf1.WindowStyle = 'docked'; 95 | 96 | % plot and animated line for position 97 | a1 = subplot( 1,2,1 ); 98 | title( 'Position' ); 99 | xlabel( 'Frame' ) 100 | ylabel( 'Position (m)' ) 101 | 102 | px = animatedline; 103 | px.MaximumNumPoints = 1000; 104 | px.Marker = '.'; 105 | px.LineWidth = 0.5; 106 | px.Color = [ 1 0 0 ]; 107 | 108 | py = animatedline; 109 | py.MaximumNumPoints = 1000; 110 | py.LineWidth = 0.5; 111 | py.Color = [ 0 1 0 ]; 112 | py.Marker = '.'; 113 | 114 | pz = animatedline; 115 | pz.MaximumNumPoints = 1000; 116 | pz.LineWidth = 0.5; 117 | pz.Color = [ 0 0 1 ]; 118 | pz.Marker = '.'; 119 | 120 | % plot and animated line for rotation 121 | a2 = subplot( 1,2,2 ); 122 | title( 'Rotation' ); 123 | xlabel( 'Frame' ) 124 | ylabel( 'Rotation (deg)' ) 125 | 126 | rx=animatedline; 127 | rx.MaximumNumPoints = 1000; 128 | rx.Marker = '.'; 129 | rx.LineWidth = 0.5; 130 | rx.Color = [ 1 0 0 ]; 131 | 132 | ry = animatedline; 133 | ry.MaximumNumPoints = 1000; 134 | ry.Marker = '.'; 135 | ry.LineWidth = 0.5; 136 | ry.Color = [ 0 1 0 ]; 137 | 138 | rz = animatedline; 139 | rz.MaximumNumPoints = 1000; 140 | rz.Marker = '.'; 141 | rz.LineWidth = 0.5; 142 | rz.Color = [ 0 0 1]; 143 | end 144 | 145 | 146 | -------------------------------------------------------------------------------- /Experimental designs/plot_data_single_rover.m: -------------------------------------------------------------------------------- 1 | %% Color matrix for plotiing data 2 | color_m=[[0.4660, 0.6740, 0.1880]; [0, 0.4470, 0.7410]; [0.9290, 0.6940, 0.1250]; [0.25, 0.25, 0.25]; [0.4940, 0.1840, 0.5560]; [0.6350, 0.0780, 0.1840]]; 3 | 4 | %% Load the dataset you want to check 5 | load('V1.mat'); 6 | 7 | %% Some basic parameters 8 | total_time = 60; 9 | time_step = 0.008; 10 | 11 | length = total_time/time_step + 1; 12 | 13 | %% Processing 14 | x_tilde = agent_pos - x_hat; 15 | x_tilde_norm = sqrt(x_tilde(:,1).^2 + x_tilde(:,2).^2 + x_tilde(:,3).^2); 16 | delta_norm = sqrt(delta_x(:,1).^2 + delta_x(:,2).^2 + delta_x(:,3).^2); 17 | beta_sat_norm = sqrt(beta_sat(:,1).^2 + beta_sat(:,2).^2 + beta_sat(:,3).^2); 18 | 19 | %% Tracking error 20 | figure(1) 21 | hold on; 22 | grid on; 23 | box on; 24 | set(gcf, 'Position', [100,100,800,400]); 25 | plot(time_matrix(1:length), x_tilde_norm, '--', 'color',color_m(1, :), 'linewidth', 2); 26 | plot(time_matrix(1:length), delta_norm, '-', 'color',color_m(2, :), 'linewidth', 2); 27 | plot(time_matrix(1:length), beta_sat_norm, '-.', 'color',color_m(3, :), 'linewidth', 2); 28 | xlim([0 60]); 29 | set(gca,'FontSize',18); 30 | set(gca,'fontname','times'); 31 | xlabel('Time (Second)','FontSize',18); 32 | ylabel('Vector norm','FontSize',18); 33 | 34 | legend("$\|\widetilde{x}\|$","$\|\delta_x\|$","$\|\beta_{\rm sat}\|$",'fontsize',18,'Orientation','horizontal','interpreter','latex'); 35 | 36 | axes('Position',[0.26,0.415,0.51625,0.3475]); 37 | hold on; 38 | grid on; 39 | box on; 40 | set(gca,'FontSize',14); 41 | set(gca, 'fontname', 'times'); 42 | % ylim([0 0.008]); 43 | % yticks(0:0.004:0.008); 44 | xlim([40 60]); 45 | 46 | plot(time_matrix(1:length), x_tilde_norm, '--', 'color',color_m(1, :), 'linewidth', 2); 47 | plot(time_matrix(1:length), delta_norm, '-', 'color',color_m(2, :), 'linewidth', 2); 48 | plot(time_matrix(1:length), beta_sat_norm, '-.', 'color',color_m(3, :), 'linewidth', 2); 49 | 50 | %% Event trigger threshold and observation error 51 | figure(2) 52 | hold on; 53 | grid on; 54 | box on; 55 | set(gcf, 'Position', [100,100,800,400]); 56 | plot(time_matrix, E_norm, '-', 'color',color_m(1, :), 'linewidth', 1.5); 57 | plot(time_matrix, trigger_bound(:,1), '--', 'color',color_m(2, :), 'linewidth', 1); 58 | plot(time_matrix, trigger_bound(:,2), '-.', 'color',color_m(3, :), 'linewidth', 1); 59 | plot(time_matrix, trigger_bound(:,3), '-.', 'color',color_m(4, :), 'linewidth', 1); 60 | xlim([0 60]); 61 | set(gca,'FontSize',18); 62 | set(gca,'fontname','times'); 63 | xlabel('Time (Second)','FontSize',18); 64 | ylabel('Function values','FontSize',18); 65 | 66 | legend("$\|E_x\|$","$F_2 - \gamma_2 \beta_{\rm est}$","$F_3 - \gamma_2 \beta_{\rm con}$",'$F_4$','fontsize',18,'Orientation','horizontal','interpreter','latex'); 67 | 68 | axes('Position',[0.22125,0.275,0.51625,0.3475]); 69 | hold on; 70 | grid on; 71 | box on; 72 | set(gca,'FontSize',14); 73 | set(gca, 'fontname', 'times'); 74 | ylim([0 0.04]); 75 | % yticks(0:0.004:0.008); 76 | xlim([50 60]); 77 | 78 | set(gca,'FontSize',18); 79 | set(gca,'fontname','times'); 80 | xlabel('Time (Second)','FontSize',18); 81 | ylabel('Function values','FontSize',18); 82 | 83 | plot(time_matrix, E_norm, '-', 'color',color_m(1, :), 'linewidth', 1.5); 84 | plot(time_matrix, trigger_bound(:,1), '--', 'color',color_m(2, :), 'linewidth', 1); 85 | plot(time_matrix, trigger_bound(:,2), '-.', 'color',color_m(3, :), 'linewidth', 1); 86 | 87 | %% Trigger interval 88 | prev_trigger = 1; 89 | figure(3) 90 | hold on; 91 | grid on; 92 | box on; 93 | set(gcf, 'Position', [100,100,1000,400]); 94 | plot([0 60], [8, 8], 'r--'); 95 | 96 | for i = 2 : size(time_matrix,1) 97 | if trigger_flag(i) == 1 98 | interval = (i - prev_trigger) * time_step * 1000; 99 | plot( (i-1) * time_step, interval, 'b*'); 100 | prev_trigger = i; 101 | end 102 | end 103 | legend("Time Trigger","Event trigger",'fontsize',18,'Orientation','vertical'); 104 | 105 | set(gca,'FontSize',18); 106 | set(gca,'fontname','times'); 107 | xlabel('Time (Second)','FontSize',18); 108 | ylabel('Trigger interval (ms)','FontSize',18); 109 | 110 | 111 | %% System trajectory 112 | figure(4) 113 | hold on; 114 | grid on; 115 | box on; 116 | set(gcf, 'Position', [100,100,600,500]); 117 | plot(pos_ref(:,1),pos_ref(:,2),'color', color_m(1,:), 'linewidth', 1.5); 118 | plot(agent_pos(:,1),agent_pos(:,2),'--','color', color_m(2,:), 'linewidth', 2); 119 | set(gca,'FontSize',18); 120 | set(gca,'fontname','times'); 121 | xlabel('X (m)','FontSize',18); 122 | ylabel('Y (m)','FontSize',18); 123 | 124 | legend("Actual","Expected",'fontsize',18,'Orientation','vertical'); 125 | 126 | %% Control input 127 | figure(5) 128 | hold on; 129 | grid on; 130 | box on; 131 | set(gcf, 'Position', [100,100,800,400]); 132 | plot(time_matrix, agent_control(:,1), '-', 'color',color_m(1,:), 'linewidth', 2); 133 | plot(time_matrix, agent_control(:,2), '--', 'color',color_m(2,:), 'linewidth', 2); 134 | plot(time_matrix, agent_control(:,3), '-.', 'color',color_m(3,:), 'linewidth', 2); 135 | xlim([0 60]); 136 | set(gca,'FontSize',18); 137 | set(gca,'fontname','times'); 138 | xlabel('Time (Second)','FontSize',18); 139 | ylabel('Tracking error','FontSize',18); 140 | 141 | plot([0 60], [0.2, 0.2], '--', 'color', color_m(4,:)); 142 | plot([0 60], [-0.2, -0.2], '--', 'color', color_m(4,:)); 143 | 144 | legend("$u(1)$","$u(2)$","$u(3)$",'fontsize',18,'Orientation','horizontal','interpreter','latex'); 145 | 146 | -------------------------------------------------------------------------------- /Experimental designs/tcp_opti_2_pc_trigger.m: -------------------------------------------------------------------------------- 1 | % The initial position of the rover [1;-1.1;pi/4] 2 | % A video is also provided for you to see the initial position 3 | %% Step 0.1: Basic settings 4 | clear; 5 | clc; 6 | step_size = 8; % Control step size in milliseconds 7 | num_agent = 1; % Number of agents 8 | dimension = 3; % Dimension of system states 9 | % Radius of the rover 10 | rover_R = [0.13,0.13,0.13]; 11 | % Radius of the wheels 12 | wheel_R = [0.05,0.05,0.05]; 13 | % Motor max speed (step/s) 14 | motor_max_speed = [3000,3000,3000]; 15 | % motor step size (switch) 16 | step_size_motor = [1/4,1/4,1/4]; 17 | % gearbox amplifier constant 18 | gear_constant = [5.18,5.18,5.18]; 19 | % Saturation limitation in m/s 20 | satu_limit = 0.2 * ones(size(motor_max_speed)); 21 | 22 | % TCP IP address for each rover 23 | % rover_ip = ['172.20.10.11';'172.20.10.12';'172.20.10.13']; 24 | rover_ip = ['129.127.29.11';'129.127.29.22';'129.127.29.33']; 25 | 26 | %% Step 0.2: Set up opti-track 27 | opti = opti_class; 28 | opti.opti_set_up(); 29 | 30 | %% Step 0.3: Set up the role of PC (one agent only) 31 | pc = pc_class; 32 | rov = struct(); 33 | rov.tcp_port = []; 34 | rov.x_i = [0;0;0]; 35 | rov.x_di = [0;0;0]; 36 | rov.v_di = [0;0;0]; 37 | rov.delta_xi = [0;0;0]; 38 | rov.serial_num = 0; 39 | rov.x_hat_i = [0;0;0]; 40 | rov.w_hat_i = [0;0;0]; 41 | rov.u_hat_i = [0;0;0]; 42 | rov.x_tilde_i = [0;0;0]; 43 | rov.beta_sat = [0.1;0.1;0.1]; 44 | rov.beta_con = 2; 45 | rov.beta_est = 2; 46 | rov.g_i = zeros(3,3); 47 | rov.u_i = [0;0;0]; 48 | rov.E_x = [0;0;0]; 49 | rov.trigger_bound = [0;0;0]; 50 | rov.x_i_trigger = [0;0;0]; % Triggered measurement 51 | 52 | %% Step 1: Wireless communication construction 53 | 54 | for i = 1 : num_agent 55 | % UDP communication 56 | pc.udp_communication(rover_ip(i,:), step_size); 57 | pause(1); 58 | % TCP communication 59 | r = rover_class; 60 | r.open_port(); 61 | rov.tcp_port = r.tcp_port; 62 | r.askID(); 63 | rov.serial_num = r.id; 64 | % Wait for a period of time 65 | pause(2); 66 | end 67 | 68 | fprintf("Communication construction is completed.\n"); 69 | 70 | %% Step 2: Real-time networked control 71 | %% Step 2.1: Some basic initialisation 72 | pause(3); 73 | stop_time = 60; % Task time (second) 74 | 75 | % Step 2.2: Data matrix initialisation 76 | % Matrix initialisation 77 | time_matrix = zeros(stop_time/step_size*1000+1,1); 78 | 79 | % Agent position read from Opti-track 80 | agent_pos = zeros(stop_time/step_size*1000+1, num_agent * dimension); 81 | agent_pos_trigger = zeros(stop_time/step_size*1000+1, num_agent * dimension); 82 | current_agent_pos = zeros(num_agent * dimension, 1); % Column vector 83 | current_agent_pos_trigger = zeros(num_agent * dimension, 1); 84 | 85 | % Reference generated by the PC 86 | pos_ref = zeros(stop_time/step_size*1000+1, num_agent * dimension); 87 | vel_ref = zeros(stop_time/step_size*1000+1, num_agent * dimension); 88 | current_pos_ref = zeros(num_agent * dimension, 1); % Column vector 89 | current_vel_ref = zeros(num_agent * dimension, 1); % Column vector 90 | 91 | % Position tracking error calculated by the PC 92 | delta_x = zeros(stop_time/step_size*1000+1, num_agent * dimension); 93 | delta_x_trigger = zeros(stop_time/step_size*1000+1, num_agent * dimension); 94 | current_delta_x = zeros(num_agent * dimension, 1); % Column vector 95 | current_delta_x_trigger = zeros(num_agent * dimension, 1); % Column vector 96 | 97 | % Observer states 98 | w_hat = zeros(stop_time/step_size*1000+1, num_agent * dimension); 99 | x_hat = zeros(stop_time/step_size*1000+1, num_agent * dimension); 100 | u_hat = zeros(stop_time/step_size*1000+1, num_agent * dimension); 101 | 102 | % Observer errors 103 | x_tilde = zeros(stop_time/step_size*1000+1, num_agent * dimension); 104 | x_tilde_trigger = zeros(stop_time/step_size*1000+1, num_agent * dimension); 105 | current_x_tilde = zeros(num_agent * dimension, 1); 106 | current_x_tilde_trigger = zeros(num_agent * dimension, 1); 107 | 108 | % Auxiliary variable 109 | beta_sat = zeros(stop_time/step_size*1000+1, num_agent * dimension); 110 | beta_sat(1, 0 * dimension + 1 : 1 * dimension) = rov.beta_sat'; 111 | 112 | % Control input calculated by the PC (only for networked control structure) 113 | agent_control = zeros(stop_time/step_size*1000+1, num_agent * dimension); 114 | motor_speed = zeros(stop_time/step_size*1000+1, num_agent * dimension); 115 | current_agent_control = zeros(num_agent * dimension, 1); % Column vector 116 | current_motor_speed = zeros(num_agent * dimension, 1); % Column vector 117 | 118 | % Event-trigger items 119 | E_norm = zeros(stop_time/step_size*1000+1, 1); 120 | beta_con = zeros(stop_time/step_size*1000+1, 1); 121 | beta_est = zeros(stop_time/step_size*1000+1, 1); 122 | beta_con(1) = rov.beta_con; 123 | beta_est(1) = rov.beta_est; 124 | trigger_bound = zeros(stop_time/step_size*1000+1, 3); 125 | trigger_flag = zeros(stop_time/step_size*1000+1, num_agent); 126 | event_num = 0; 127 | 128 | % Record the calculation time 129 | agent_sta_time = zeros(stop_time/step_size*1000+1, num_agent); 130 | agent_end_time = zeros(stop_time/step_size*1000+1, num_agent); 131 | 132 | % Event-trigger parameters 133 | eta_1 = [21,21,3]; 134 | k_1 = [3,3,1]; 135 | xi_2 = 0.5; 136 | rho_2 = 0.08; 137 | gamma_2 = 0.6; 138 | alpha_2 = 1; 139 | 140 | % Controller 141 | k_2 = [3,3,1.5]; 142 | k_3 = [8,8,2]; 143 | rho_3 = 0.08; 144 | xi_3 = 0.5; 145 | gamma_3 = 0.6; 146 | alpha_3 = 1; 147 | rho_4 = 0.08; 148 | L_1 = 1.74; 149 | L_2 = 1.42; 150 | 151 | % Record the initial time 152 | timer = clock; 153 | ini_time = (timer(4) * 3600 + timer(5) * 60 + timer(6)) * 1000; 154 | 155 | % Now we entre the big loop 156 | int_num = 0; % The number of control iteration 157 | rov.beta_sat = [0.1;0.1;0.1]; 158 | rov.beta_con = 2; 159 | rov.beta_est = 2; 160 | % Step 2.3: Begin the big real-time control loop 161 | while 1 162 | 163 | % Step 2.3.1: Record the current time 164 | timer = clock; 165 | cur_time = (timer(4) * 3600 + timer(5) * 60 + timer(6)) * 1000; 166 | now_time = cur_time - ini_time; % Unit: ms 167 | 168 | % Step 2.3.2: If the time is triggered 169 | if now_time >= step_size * int_num 170 | time_matrix(int_num + 1) = now_time/1000; 171 | % Step 2.3.2.1: Reference generation and position reading 172 | for i = 1 : num_agent 173 | % Record start time 174 | timer = clock; 175 | start_time = (timer(4) * 3600 + timer(5) * 60 + timer(6)) * 1000 - ini_time; 176 | 177 | % Generate reference 178 | [rov.x_di, rov.v_di] = pc.reference_generator(now_time/1000, i); 179 | 180 | % Read current position from opti-track 181 | rov.x_i = opti.opti_get_pos(i); 182 | % Initialise 183 | if int_num == 0 184 | rov.x_hat_i = rov.x_i; 185 | rov.x_i_trigger = rov.x_hat_i; 186 | current_agent_pos_trigger = rov.x_i_trigger; 187 | trigger_flag(1) = 1; 188 | E_x_norm = 0; 189 | event_num = event_num + 1; 190 | rov.delta_xi = rov.x_i - rov.x_di; 191 | rov.x_tilde_i = [0;0;0]; 192 | rov.E_x = [0;0;0]; 193 | K_3 = diag(eta_1) * diag(k_1) - 2 * eye(3); 194 | F_2 = (L_1 * norm(diag(eta_1) * rov.u_i)^2 * E_x_norm^2/4 + norm(diag(eta_1)* diag(k_1) * rov.E_x)^2/4 + norm(diag(eta_1) * rov.E_x)^2) - rho_2 * rov.x_tilde_i' * K_3 * rov.x_tilde_i; 195 | F_3 = (L_2 * norm(rov.v_di - rov.w_hat_i - diag(k_3) * rov.beta_sat)^2 * E_x_norm^2 + norm(diag(k_2) * rov.E_x)^2) - rho_3 * rov.delta_xi' * diag(k_2) * rov.delta_xi; 196 | F_4 = max(k_2) * L_2 * E_x_norm * norm(rov.delta_xi)^2 - rho_4 * (1 - rho_3) * norm(diag(k_2) * rov.delta_xi.^2); 197 | 198 | rov.trigger_bound = [F_2 - gamma_2 * rov.beta_est; F_3 - gamma_3 * rov.beta_con; F_4]; 199 | 200 | rov.beta_est = rov.beta_est + step_size/1000 * (- alpha_2 * rov.beta_est - F_2 + xi_2); 201 | rov.beta_con = rov.beta_con + step_size/1000 * (- alpha_3 * rov.beta_con - F_3 + xi_3); 202 | end 203 | rov.delta_xi = rov.x_i - rov.x_di; 204 | rov.x_tilde_i = rov.x_hat_i - rov.x_i; 205 | rov.E_x = rov.x_i - rov.x_i_trigger; 206 | 207 | % Temporary storage 208 | current_pos_ref((i-1) * dimension + 1 : i * dimension) = rov.x_di; 209 | current_vel_ref((i-1) * dimension + 1 : i * dimension) = rov.v_di; 210 | current_agent_pos((i-1) * dimension + 1 : i * dimension) = rov.x_i; 211 | current_delta_x((i-1) * dimension + 1 : i * dimension) = rov.delta_xi; 212 | current_x_tilde((i-1) * dimension + 1 : i * dimension) = rov.x_tilde_i; 213 | 214 | if int_num > 0 % We do nothing for t_0 215 | 216 | % Event-trigger mechanism 217 | E_x_norm = norm(rov.E_x); 218 | K_3 = diag(eta_1) * diag(k_1) - 2 * eye(3); 219 | F_2 = (L_1 * norm(diag(eta_1) * rov.u_i)^2 * E_x_norm^2/4 + norm(diag(eta_1)* diag(k_1) * rov.E_x)^2/4 + norm(diag(eta_1) * rov.E_x)^2) - rho_2 * rov.x_tilde_i' * K_3 * rov.x_tilde_i; 220 | F_3 = (L_2 * norm(rov.v_di - rov.w_hat_i - diag(k_3) * rov.beta_sat)^2 * E_x_norm^2 + norm(diag(k_2) * rov.E_x)^2) - rho_3 * rov.delta_xi' * diag(k_2) * rov.delta_xi; 221 | F_4 = max(k_2) * L_2 * E_x_norm * norm(rov.delta_xi)^2 - rho_4 * (1 - rho_3) * norm(diag(k_2) * rov.delta_xi.^2); 222 | 223 | rov.trigger_bound = [F_2 - gamma_2 * rov.beta_est; F_3 - gamma_3 * rov.beta_con; F_4]; 224 | 225 | if max(rov.trigger_bound) > 0 226 | trigger_flag(int_num+1) = 1; 227 | rov.x_i_trigger = rov.x_i; 228 | E_x_norm = 0; 229 | event_num = event_num + 1; 230 | F_2 = (L_1 * norm(diag(eta_1) * rov.u_i)^2 * E_x_norm^2/4 + norm(diag(eta_1)* diag(k_1) * rov.E_x)^2/4 + norm(diag(eta_1) * rov.E_x)^2) - rho_2 * rov.x_tilde_i' * K_3 * rov.x_tilde_i; 231 | F_3 = (L_2 * norm(rov.v_di - rov.w_hat_i - diag(k_3) * rov.beta_sat)^2 * E_x_norm^2 + norm(diag(k_2) * rov.E_x)^2) - rho_3 * rov.delta_xi' * diag(k_2) * rov.delta_xi; 232 | else 233 | trigger_flag(int_num+1) = 0; 234 | end 235 | 236 | % Update of the dynamic threshold 237 | rov.beta_est = rov.beta_est + step_size/1000 * (- alpha_2 * rov.beta_est - F_2 + xi_2); 238 | rov.beta_con = rov.beta_con + step_size/1000 * (- alpha_3 * rov.beta_con - F_3 + xi_3); 239 | 240 | % If we want to use time trigger mechanism only 241 | % trigger_flag(int_num+1) = 1; 242 | % rov.x_i_trigger = rov.x_i; 243 | % E_x_norm = 0; 244 | % event_num = event_num + 1; 245 | 246 | current_agent_pos_trigger = rov.x_i_trigger; 247 | 248 | % Uncertainty observation 249 | rov = pc.uncertainty_observer(step_size/1000, rover_R(i), rov); 250 | 251 | % Adaptive observer 252 | rov = pc.controller(step_size/1000, satu_limit(i), rover_R(i), rov); 253 | 254 | % Communication to Arduino 255 | r.sendreference(rov.u_i, rov.tcp_port); 256 | 257 | end 258 | 259 | % Data recording 260 | agent_control(int_num+1, (i-1) * dimension + 1 : i * dimension) = rov.u_i'; 261 | beta_sat(int_num+1, (i-1) * dimension + 1 : i * dimension) = rov.beta_sat'; 262 | x_hat(int_num+1, (i-1) * dimension + 1 : i * dimension) = rov.x_hat_i'; 263 | w_hat(int_num+1, (i-1) * dimension + 1 : i * dimension) = rov.w_hat_i'; 264 | u_hat(int_num+1, (i-1) * dimension + 1 : i * dimension) = rov.u_hat_i'; 265 | E_norm(int_num+1) = E_x_norm; 266 | beta_est(int_num+1) = rov.beta_est; 267 | beta_con(int_num+1) = rov.beta_con; 268 | 269 | % Record end time 270 | timer = clock; 271 | end_time = (timer(4) * 3600 + timer(5) * 60 + timer(6)) * 1000 - ini_time; 272 | 273 | agent_sta_time(int_num+1, i) = start_time; 274 | agent_end_time(int_num+1, i) = end_time; 275 | 276 | end 277 | 278 | pos_ref(int_num+1, :) = current_pos_ref'; 279 | vel_ref(int_num+1, :) = current_vel_ref'; 280 | agent_pos(int_num+1, :) = current_agent_pos'; 281 | delta_x(int_num+1, :) = current_delta_x'; 282 | agent_pos_trigger(int_num+1, :) = current_agent_pos_trigger'; 283 | trigger_bound(int_num + 1, :) = rov.trigger_bound'; 284 | 285 | % Propagation recording 286 | int_num = int_num + 1; 287 | 288 | end 289 | 290 | if now_time > stop_time * 1000 291 | for i = 1 : num_agent 292 | r.sendreference(zeros(3,1), rov.tcp_port); 293 | end 294 | break; 295 | end 296 | end 297 | 298 | 299 | %% In case of emergency, run this to stop the system 300 | for i = 1 : num_agent 301 | r.sendreference(zeros(3,1), rov.tcp_port); 302 | end 303 | 304 | %% -------------------------------------------------------------------------------- /Simulink files/Paper_8_plot.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear; 3 | 4 | color_m=[[0.4660, 0.6740, 0.1880]; [0, 0.4470, 0.7410]; [0.9290, 0.6940, 0.1250]; [0.25, 0.25, 0.25]; [0.4940, 0.1840, 0.5560]; [0.6350, 0.0780, 0.1840]]; 5 | 6 | %% Time trigger + estimator 7 | load("V1.mat"); 8 | 9 | ref_x = dilute_data(out.ref_x1,10); 10 | time = dilute_data(out.tout, 10); 11 | 12 | delta_x_1 = dilute_data(out.delta_x, 10); 13 | u_1 = dilute_data(out.u, 10); 14 | w_tilde_norm_1 = dilute_data(out.w_tilde_norm, 10); 15 | x_tilde_norm_1 = dilute_data(out.x_tilde_norm, 10); 16 | delta_x_norm_1 = dilute_data(out.delta_x_norm, 10); 17 | x_1 = dilute_data(out.x, 10); 18 | 19 | %% Time trigger + ESO 20 | load("V2.mat"); 21 | 22 | delta_x_2 = dilute_data(out.delta_x, 10); 23 | u_2 = dilute_data(out.u, 10); 24 | w_tilde_norm_2 = dilute_data(out.w_tilde_norm, 10); 25 | x_tilde_norm_2 = dilute_data(out.x_tilde_norm, 10); 26 | delta_x_norm_2 = dilute_data(out.delta_x_norm, 10); 27 | x_2 = dilute_data(out.x, 10); 28 | 29 | 30 | %% Event trigger estimator 31 | load("V3.mat"); 32 | 33 | delta_x_3 = dilute_data(out.delta_x, 10); 34 | u_3 = dilute_data(out.u, 10); 35 | w_tilde_norm_3 = dilute_data(out.w_tilde_norm, 10); 36 | x_tilde_norm_3 = dilute_data(out.x_tilde_norm, 10); 37 | delta_x_norm_3 = dilute_data(out.delta_x_norm, 10); 38 | x_3 = dilute_data(out.x, 10); 39 | 40 | scale = 1; 41 | time_trigger = dilute_data(out.tout, scale); 42 | beta_trigger_3 = dilute_data(structure_to_array(out.beta_trigger), scale); 43 | trigger_flag_3 = dilute_data(out.trigger_flag, scale); 44 | trigger_num_3 = dilute_data(out.trigger_num, scale); 45 | trigger_threshold_3 = dilute_data(structure_to_array(out.trigger_threshold), scale); 46 | E_x_norm_3 = dilute_data(out.trigger_threshold.signals(2).values, scale); 47 | beta_sat_3 = dilute_data(structure_to_array(out.beta_sat), 10); 48 | beta_sat_norm_3 = sqrt(beta_sat_3(:,1).^2+beta_sat_3(:,2).^2+beta_sat_3(:,3).^2); 49 | 50 | 51 | %% Event trigger + ESO 52 | load("V4.mat"); 53 | 54 | delta_x_4 = dilute_data(out.delta_x, 10); 55 | u_4 = dilute_data(out.u, 10); 56 | w_tilde_norm_4 = dilute_data(out.w_tilde_norm, 10); 57 | x_tilde_norm_4 = dilute_data(out.x_tilde_norm, 10); 58 | delta_x_norm_4 = dilute_data(out.delta_x_norm, 10); 59 | x_4 = dilute_data(out.x, 10); 60 | 61 | beta_trigger_4 = dilute_data(structure_to_array(out.beta_trigger), scale); 62 | trigger_flag_4 = dilute_data(out.trigger_flag, scale); 63 | trigger_num_4 = dilute_data(out.trigger_num, scale); 64 | trigger_threshold_4 = dilute_data(structure_to_array(out.trigger_threshold), scale); 65 | E_x_norm_4 = dilute_data(out.trigger_threshold.signals(2).values, scale); 66 | beta_sat_4 = dilute_data(structure_to_array(out.beta_sat), 10); 67 | beta_sat_norm_4 = sqrt(beta_sat_4(:,1).^2+beta_sat_4(:,2).^2+beta_sat_4(:,3).^2); 68 | 69 | %% Time to plot 70 | 71 | %% x_tilde_norm 72 | figure(1) 73 | set(gcf,'Position',[-100,-200,700,1400]); 74 | subplot(3,1,1) 75 | hold on; 76 | grid on; 77 | box on; 78 | plot(time,x_tilde_norm_1(:,2),'--','color',color_m(1,:), 'linewidth', 1.5); 79 | plot(time,x_tilde_norm_2(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 80 | plot(time,x_tilde_norm_3(:,2),'-','color',color_m(3,:), 'linewidth', 1.5); 81 | plot(time,x_tilde_norm_4(:,2),'--','color',color_m(4,:), 'linewidth', 1.5); 82 | ylim([0, 0.04]); 83 | yticks([0:0.01:0.04]); 84 | set(gca,'FontSize',16); 85 | set(gca,'FontName','times'); 86 | xlabel('Time (Second)','FontSize',16); 87 | ylabel('$\|\widetilde{x}\|$','FontSize',16,'interpreter','latex'); 88 | % legend('TTEBC','TTOBC','ETEBC','ETOBC','fontsize',16,'Orientation','horizontal','POsition',[0.14,0.9517,0.7502,0.0372]); 89 | 90 | 91 | subplot(3,1,2) 92 | hold on; 93 | grid on; 94 | box on; 95 | plot(time,w_tilde_norm_1(:,2),'--','color',color_m(1,:), 'linewidth', 1.5); 96 | plot(time,w_tilde_norm_2(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 97 | plot(time,w_tilde_norm_3(:,2),'-','color',color_m(3,:), 'linewidth', 1.5); 98 | plot(time,w_tilde_norm_4(:,2),'--','color',color_m(4,:), 'linewidth', 1.5); 99 | set(gca,'FontSize',16); 100 | set(gca,'FontName','times'); 101 | ylim([0, 0.12]); 102 | yticks([0 0.04 0.08 0.12]); 103 | xlabel('Time (Second)','FontSize',16); 104 | ylabel('$\|\widetilde{w}\|$','FontSize',16,'interpreter','latex'); 105 | 106 | axes('Position',[0.26,0.502522704339052,0.518333333333333,0.100908173562059]); 107 | grid on; 108 | box on; 109 | hold on; 110 | set(gca,'FontSize',13); 111 | set(gca,'FontName','times'); 112 | plot(time,w_tilde_norm_1(:,2),'--','color',color_m(1,:), 'linewidth', 1.5); 113 | plot(time,w_tilde_norm_2(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 114 | plot(time,w_tilde_norm_3(:,2),'-','color',color_m(3,:), 'linewidth', 1.5); 115 | plot(time,w_tilde_norm_4(:,2),'--','color',color_m(4,:), 'linewidth', 1.5); 116 | xlim([40 60]); 117 | ylim([0 0.024]); 118 | yticks([0 0.006 0.012 0.018 0.024]); 119 | 120 | subplot(3,1,3) 121 | hold on; 122 | grid on; 123 | box on; 124 | plot(time,delta_x_norm_1(:,2),'--','color',color_m(1,:), 'linewidth', 1.5); 125 | plot(time,delta_x_norm_2(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 126 | plot(time,delta_x_norm_3(:,2),'-','color',color_m(3,:), 'linewidth', 1.5); 127 | plot(time,delta_x_norm_4(:,2),'--','color',color_m(4,:), 'linewidth', 1.5); 128 | set(gca,'FontSize',16); 129 | set(gca,'FontName','times'); 130 | ylim([0, 1.2]); 131 | yticks([0:0.3:1.2]); 132 | xlabel('Time (Second)','FontSize',16); 133 | ylabel('$\|\delta_x\|$','FontSize',16,'interpreter','latex'); 134 | legend('TTEBC','TTOBC','ETEBC','ETOBC','fontsize',16,'Orientation','horizontal','POsition',[0.14,0.9517,0.702,0.02]); 135 | 136 | axes('Position',[0.258666666666667,0.187941473259334,0.519,0.109737638748739]); 137 | grid on; 138 | box on; 139 | hold on; 140 | set(gca,'FontSize',13); 141 | set(gca,'FontName','times'); 142 | plot(time,delta_x_norm_1(:,2),'--','color',color_m(1,:), 'linewidth', 1.5); 143 | plot(time,delta_x_norm_2(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 144 | plot(time,delta_x_norm_3(:,2),'-','color',color_m(3,:), 'linewidth', 1.5); 145 | plot(time,delta_x_norm_4(:,2),'--','color',color_m(4,:), 'linewidth', 1.5); 146 | xlim([40 60]); 147 | ylim([0 0.015]); 148 | yticks([0 0.005 0.01 0.015]); 149 | 150 | 151 | %% Trigger time and event numbers 152 | reu_3 =[]; 153 | prev_event = 1; 154 | % First, some data processing 155 | step_length = 0.008; 156 | for i = 2 : size(time_trigger,1) 157 | if trigger_flag_3(i,2) == 1 158 | temp = [(i-1)*step_length,(i - prev_event)*step_length*1000]; 159 | reu_3 = [reu_3;temp]; 160 | prev_event = i; 161 | end 162 | end 163 | 164 | reu_4 =[]; 165 | prev_event = 1; 166 | % First, some data processing 167 | step_length = 0.008; 168 | for i = 2 : size(time_trigger,1) 169 | if trigger_flag_4(i,2) == 1 170 | temp = [(i-1)*step_length,(i - prev_event)*step_length*1000]; 171 | reu_4 = [reu_4;temp]; 172 | prev_event = i; 173 | end 174 | end 175 | 176 | % Then, we plot 177 | figure(2) 178 | set(gcf,'Position',[-100,-200,700,1400]); 179 | subplot(3,1,1) 180 | hold on; 181 | grid on; 182 | box on; 183 | plot([0,60],[step_length*1000,step_length*1000],'--','color',color_m(3,:)); 184 | for i = 1:size(reu_3,1) 185 | plot(reu_3(i,1),reu_3(i,2),'*','color',color_m(1,:)); 186 | end 187 | set(gca,'FontSize',16); 188 | set(gca,'FontName','times'); 189 | ylim([0 330]); 190 | yticks(0:110:330); 191 | xlabel('Time (Second)','FontSize',16); 192 | ylabel('Trigger interval (ms)','FontSize',16,'interpreter','latex'); 193 | legend('Time-triggered (8ms)','ETEBC','fontsize',16,'Orientation','horizontal'); 194 | 195 | subplot(3,1,2) 196 | hold on; 197 | grid on; 198 | box on; 199 | plot([0,60],[step_length*1000,step_length*1000],'--','color',color_m(3,:)); 200 | for i = 1:size(reu_4,1) 201 | plot(reu_4(i,1),reu_4(i,2),'*','color',color_m(2,:)); 202 | end 203 | set(gca,'FontSize',16); 204 | set(gca,'FontName','times'); 205 | xlabel('Time (Second)','FontSize',16); 206 | ylim([0 330]); 207 | yticks(0:110:330); 208 | ylabel('Trigger interval (ms)','FontSize',16,'interpreter','latex'); 209 | legend('Time-triggered (8ms)','ETOBC','fontsize',16,'Orientation','horizontal'); 210 | 211 | 212 | subplot(3,1,3) 213 | hold on; 214 | grid on; 215 | box on; 216 | plot(dilute_data(time_trigger,10),dilute_data(trigger_num_3(:,2),10),'--','color',color_m(1,:), 'linewidth', 1.5); 217 | plot(dilute_data(time_trigger,10),dilute_data(trigger_num_4(:,2),10),'-.','color',color_m(2,:), 'linewidth', 1.5); 218 | set(gca,'FontSize',16); 219 | set(gca,'FontName','times'); 220 | xlabel('Time (Second)','FontSize',16); 221 | ylabel('Event number','FontSize',16); 222 | ylim([0 500]); 223 | yticks(0:100:500); 224 | legend('ETEBC','ETOBC','fontsize',16,'Orientation','horizontal'); 225 | 226 | axes('Position',[0.572571428571429,0.164434914228052,0.293714285714286,0.0686175580222]); 227 | grid on; 228 | box on; 229 | hold on; 230 | set(gca,'FontSize',13); 231 | set(gca,'FontName','times'); 232 | plot(dilute_data(time_trigger,2),dilute_data(trigger_num_3(:,2),2),'--','color',color_m(1,:), 'linewidth', 1.5); 233 | plot(dilute_data(time_trigger,2),dilute_data(trigger_num_4(:,2),2),'-.','color',color_m(2,:), 'linewidth', 1.5); 234 | set(gca,'FontSize',13); 235 | set(gca,'FontName','times'); 236 | xlim([59 60]); 237 | 238 | 239 | 240 | 241 | 242 | %% Trigger thresholds 243 | figure(3) 244 | set(gcf,'Position',[-100,-200,700,1400]); 245 | subplot(2,1,1) 246 | hold on; 247 | grid on; 248 | box on; 249 | set(gca,'FontName','times'); 250 | set(gca,'FontSize',16); 251 | plot(time_trigger,trigger_threshold_3(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 252 | plot(time_trigger,trigger_threshold_3(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 253 | plot(time_trigger,trigger_threshold_3(:,3),'--','color',color_m(3,:), 'linewidth', 1.5); 254 | plot(time_trigger,E_x_norm_3,'-','color',color_m(4,:), 'linewidth', 1.5); 255 | ylim([-1.6 0.4]); 256 | yticks(-1.6:0.4:0.4); 257 | xlabel('Time (Second)','FontSize',16); 258 | ylabel('Vector norm','FontSize',16); 259 | legend('$F_2 - \alpha_2 \beta_{\rm est}$','$F_3 - \alpha_3 \beta_{\rm con}$','$F_4$','$\|E_x\|$','fontsize',16,'Orientation','horizontal','POsition',[0.14,0.9517,0.702,0.02],'interpreter','latex'); 260 | title("ETEBC"); 261 | 262 | 263 | axes('Position',[0.259428571428571,0.641775983854692,0.518571428571429,0.161453077699294]); 264 | grid on; 265 | box on; 266 | hold on; 267 | set(gca,'FontSize',13); 268 | set(gca,'FontName','times'); 269 | plot(time_trigger,trigger_threshold_3(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 270 | plot(time_trigger,trigger_threshold_3(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 271 | plot(time_trigger,trigger_threshold_3(:,3),'--','color',color_m(3,:), 'linewidth', 1.5); 272 | plot(time_trigger,E_x_norm_3,'-','color',color_m(4,:), 'linewidth', 1.5); 273 | xlim([50 60]); 274 | ylim([0 0.05]); 275 | yticks(0:0.01:0.05); 276 | 277 | 278 | subplot(2,1,2) 279 | hold on; 280 | grid on; 281 | box on; 282 | set(gca,'FontName','times'); 283 | set(gca,'FontSize',16); 284 | plot(time_trigger,trigger_threshold_4(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 285 | plot(time_trigger,trigger_threshold_4(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 286 | plot(time_trigger,trigger_threshold_4(:,3),'--','color',color_m(3,:), 'linewidth', 1.5); 287 | plot(time_trigger,E_x_norm_4,'-','color',color_m(4,:), 'linewidth', 1.5); 288 | ylim([-1.6 0.4]); 289 | yticks(-1.6:0.4:0.4); 290 | xlabel('Time (Second)','FontSize',16); 291 | ylabel('Vector norm','FontSize',16); 292 | legend('$F_1 - \alpha_1 \beta_{\rm eso}$','$F_3 - \alpha_3 \beta_{\rm con}$','$F_4$','$\|E_x\|$','fontsize',16,'Orientation','horizontal','POsition',[0.14,0.4717,0.702,0.02],'interpreter','latex'); 293 | title("ETOBC"); 294 | 295 | axes('Position',[0.259428571428571,0.141775983854692,0.518571428571429,0.161453077699294]); 296 | grid on; 297 | box on; 298 | hold on; 299 | set(gca,'FontSize',13); 300 | set(gca,'FontName','times'); 301 | plot(time_trigger,trigger_threshold_4(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 302 | plot(time_trigger,trigger_threshold_4(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 303 | plot(time_trigger,trigger_threshold_4(:,3),'--','color',color_m(3,:), 'linewidth', 1.5); 304 | plot(time_trigger,E_x_norm_4,'-','color',color_m(4,:), 'linewidth', 1.5); 305 | xlim([50 60]); 306 | ylim([0 0.05]); 307 | yticks(0:0.01:0.05); 308 | 309 | 310 | %% Tracking trajectory 311 | figure(4) 312 | set(gcf,'Position',[-50,-50,500,600]); 313 | grid on; 314 | box on; 315 | hold on; 316 | set(gca,'FontSize',13); 317 | set(gca,'FontName','times'); 318 | plot(x_3(:,2),x_3(:,3),'-','color',color_m(1,:), 'linewidth', 1.5); 319 | plot(ref_x(:,2),ref_x(:,3),'-.','color',color_m(2,:), 'linewidth', 1.5); 320 | xlabel('X (m)','FontSize',16); 321 | ylabel('Y (m)','FontSize',16); 322 | legend('ETEBC (Simulation)','Reference','fontsize',16,'Orientation','horizontal'); 323 | 324 | 325 | %% Convergence of beta 326 | figure(5) 327 | set(gcf,'Position',[-100,-200,1100,500]); 328 | grid on; 329 | box on; 330 | hold on; 331 | subplot(1,2,1) 332 | hold on; 333 | grid on; 334 | box on; 335 | set(gca,'FontName','times'); 336 | set(gca,'FontSize',16); 337 | plot(time_trigger,beta_trigger_3(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 338 | plot(time_trigger,beta_trigger_3(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 339 | plot(time,beta_sat_norm_3,'--','color',color_m(3,:), 'linewidth', 1.5); 340 | % ylim([-1.8 0.2]); 341 | % yticks(-1.8:0.2:0.2); 342 | xlabel('Time (Second)','FontSize',16); 343 | ylabel('Vector norm','FontSize',16); 344 | legend('$\beta_{\rm est}$','$\beta_{\rm con}$','$\|\beta_{\rm sat}\|$','fontsize',16,'Orientation','horizontal','interpreter','latex'); 345 | title("ETEBC"); 346 | 347 | subplot(1,2,2) 348 | hold on; 349 | grid on; 350 | box on; 351 | set(gca,'FontName','times'); 352 | set(gca,'FontSize',16); 353 | plot(time_trigger,beta_trigger_4(:,1),'--','color',color_m(1,:), 'linewidth', 1.5); 354 | plot(time_trigger,beta_trigger_4(:,2),'-.','color',color_m(2,:), 'linewidth', 1.5); 355 | plot(time,beta_sat_norm_4,'--','color',color_m(3,:), 'linewidth', 1.5); 356 | % ylim([-1.8 0.2]); 357 | % yticks(-1.8:0.2:0.2); 358 | xlabel('Time (Second)','FontSize',16); 359 | ylabel('Vector norm','FontSize',16); 360 | legend('$\beta_{\rm eso}$','$\beta_{\rm con}$','$\|\beta_{\rm sat}\|$','fontsize',16,'Orientation','horizontal','interpreter','latex'); 361 | title("ETOBC"); 362 | 363 | %% Convergence region 364 | clc 365 | 366 | time_start = 40; 367 | time_end = 60; 368 | step = 0.08; 369 | 370 | % max(x_tilde_norm_1(time_start/step:time_end/step,2)) 371 | % max(x_tilde_norm_2(time_start/step:time_end/step,2)) 372 | % max(x_tilde_norm_3(time_start/step:time_end/step,2)) 373 | % max(x_tilde_norm_4(time_start/step:time_end/step,2)) 374 | % 375 | % max(w_tilde_norm_1(time_start/step:time_end/step,2)) 376 | % max(w_tilde_norm_2(time_start/step:time_end/step,2)) 377 | % max(w_tilde_norm_3(time_start/step:time_end/step,2)) 378 | % max(w_tilde_norm_4(time_start/step:time_end/step,2)) 379 | 380 | max(delta_x_norm_1(time_start/step:time_end/step,2)) 381 | max(delta_x_norm_2(time_start/step:time_end/step,2)) 382 | max(delta_x_norm_3(time_start/step:time_end/step,2)) 383 | max(delta_x_norm_4(time_start/step:time_end/step,2)) 384 | 385 | step = 0.008; 386 | % max(E_x_norm_3(time_start/step:time_end/step)) 387 | % max(E_x_norm_4(time_start/step:time_end/step)) 388 | -------------------------------------------------------------------------------- /Experimental designs/quaternion.m: -------------------------------------------------------------------------------- 1 | classdef quaternion 2 | % classdef quaternion, implements quaternion mathematics and 3D rotations 3 | % 4 | % Properties (SetAccess = protected): 5 | % e(4,1) components, basis [1; i; j; k]: e(1) + i*e(2) + j*e(3) + k*e(4) 6 | % i*j=k, j*i=-k, j*k=i, k*j=-i, k*i=j, i*k=-j, i*i = j*j = k*k = -1 7 | % 8 | % Constructors: 9 | % q = quaternion scalar zero quaternion, q.e = [0;0;0;0] 10 | % q = quaternion(x) x is a matrix size [4,s1,s2,...] or [s1,4,s2,...], 11 | % q is size [s1,s2,...], q(i1,i2,...).e = ... 12 | % x(1:4,i1,i2,...) or x(i1,1:4,i2,...).' 13 | % q = quaternion(v) v is a matrix size [3,s1,s2,...] or [s1,3,s2,...], 14 | % q is size [s1,s2,...], q(i1,i2,...).e = ... 15 | % [0;v(1:3,i1,i2,...)] or [0;v(i1,1:3,i2,...).'] 16 | % q = quaternion(c) c is a complex matrix size [s1,s2,...], 17 | % q is size [s1,s2,...], q(i1,i2,...).e = ... 18 | % [real(c(i1,i2,...));imag(c(i1,i2,...));0;0] 19 | % q = quaternion(x1,x2) x1,x2 are matrices size [s1,s2,...] or scalars, 20 | % q(i1,i2,...).e = [x1(i1,i2,...);x2(i1,i2,...);0;0] 21 | % q = quaternion(v1,v2,v3) v1,v2,v3 matrices size [s1,s2,...] or scalars, 22 | % q(i1,i2,...).e = [0;v1(i1,i2,...);v2(i1,i2,...);... 23 | % v3(i1,i2,...)] 24 | % q = quaternion(x1,x2,x3,x4) x1,x2,x3,x4 matrices size [s1,s2,...] or scalars, 25 | % q(i1,i2,...).e = [x1(i1,i2,...);x2(i1,i2,...);... 26 | % x3(i1,i2,...);x4(i1,i2,...)] 27 | % 28 | % Quaternion array constructor methods: 29 | % q = quaternion.complexmatrix(Q) 30 | % construct quaternions from complex 2x2 matrices 31 | % q = quaternion.eye(N) quaternion NxN identity matrix 32 | % q = quaternion.nan(siz) q(:).e = [NaN;NaN;NaN;NaN] 33 | % q = quaternion.ones(siz) q(:).e = [1;0;0;0] 34 | % q = quaternion.rand(siz) uniform random quaternions, NOT normalized 35 | % to 1, 0 <= q.e(1) <= 1, -1 <= q.e(2:4) <= 1 36 | % q = quaternion.randRot(siz) random quaternions uniform in rotation space 37 | % q = quaternion.zeros(siz) q(:).e = [0;0;0;0] 38 | % 39 | % Rotation constructor methods (all lower case): 40 | % q = quaternion.angleaxis(angle,axis) 41 | % angle is an array in radians, axis is an array 42 | % of vectors size [3,s1,s2,...] or [s1,3,s2,...], 43 | % q is size [s1,s2,...], quaternions normalized to 1 44 | % equivalent to rotations about axis by angle 45 | % q = quaternion.eulerangles(axes,angles) or 46 | % q = quaternion.eulerangles(axes,ang1,ang2,ang3) 47 | % axes is a string array or cell string array, 48 | % '123' = 'xyz' = 'XYZ' = 'ijk', etc., 49 | % angles is an array of Euler angles in radians, 50 | % size [3,s1,s2,...] or [s1,3,s2,...], or 51 | % (ang1, ang2, ang3) are arrays or scalars of 52 | % Euler angles in radians, q is size 53 | % [s1,s2,...], quaternions normalized to 1 54 | % equivalent to Euler Angle rotations 55 | % q = quaternion.integrateomega(t,w,odeoptions) or 56 | % q = quaternion.integrateomega(t,omega,axis,odeoptions) 57 | % integrate angular velocities over time 58 | % q = quaternion.modifiedrodrigues(mrp) 59 | % quaternions from Modified Rodrigues parameters 60 | % q = quaternion.rodrigues(rp) 61 | % quaternions from Rodrigues parameters 62 | % q = quaternion.rotateutov(u,v,dimu,dimv) 63 | % quaternions normalized to 1 that rotate 3 64 | % element vectors u into the directions of 3 65 | % element vectors v 66 | % q = quaternion.rotationmatrix(R) 67 | % R is an array of rotation or Direction Cosine 68 | % Matrices size [3,3,s1,s2,...] with det(R) == 1, 69 | % q(i1,i2,...) = quaternions normalized to 1, 70 | % equivalent to R(1:3,1:3,i1,i2,...) 71 | % 72 | % Rotation methods (Mixed Case): 73 | % [angle,axis] = AngleAxis(q) angles in radians, unit vector rotation axes 74 | % equivalent to q 75 | % qd = Derivative(q,w) quaternion derivatives, w are 3 component 76 | % angular velocity vectors, qd = 0.5*q*quaternion(w) 77 | % angles = EulerAngles(q,axes) angles are 3 Euler angles equivalent to q, axes 78 | % are strings or cell strings, '123' = 'xyz', etc. 79 | % q1 = Integral(q0,t,w,odeoptions) or 80 | % q1 = Integral(q0,t,omega,axis,odeoptions) 81 | % integrate angular velocities to quaternions 82 | % mrp = ModifiedRodrigues(q) Modified Rodrigues parameters equivalent to q 83 | % [omega,axis] = OmegaAxis(q,t,dim) 84 | % instantaneous angular velocities and rotation axes 85 | % PlotRotation(q,interval) plot columns of rotation matrices of q, 86 | % pause interval between figure updates in seconds 87 | % [q1,w1,t1] = PropagateEulerEq(q0,w0,I,t,@torque,odeoptions) 88 | % Euler equation numerical propagator, see 89 | % help quaternion.PropagateEulerEq 90 | % rp = Rodrigues(q) Rodrigues parameters equivalent to q 91 | % Tp = RotateTensor(q,T) rotations q converted to rotation matrices R, 92 | % acting on 3x3 tensors T: Tp = R * T * R.' 93 | % vp = RotateVector(q,v,dim) vp are 3 component vectors, rotations q acting 94 | % on vectors v, uses rotation matrix multiplication 95 | % vp = RotateVectorQ(q,v,dim) vp are 3 component vectors, rotations q acting 96 | % on vectors v, uses quaternion multiplication, 97 | % RotateVector is 7 times faster than RotateVectorQ 98 | % R = RotationMatrix(q) 3x3 rotation matrices equivalent to q 99 | % 100 | % Note: 101 | % In all rotation operations, the rotations operate from left to right on 102 | % 3x1 column vectors and create rotated vectors, not representations of 103 | % those vectors in rotated coordinate systems. 104 | % For Euler angles, '123' means rotate the vector about x first, about y 105 | % second, about z third, i.e.: 106 | % vp = rotate(z,angle(3)) * rotate(y,angle(2)) * rotate(x,angle(1)) * v 107 | % 108 | % Ordinary methods: 109 | % n = abs(q) quaternion norm, n = sqrt( sum( q.e.^2 )) 110 | % q3 = bsxfun(func,q1,q2) binary singleton expansion of operation func 111 | % c = complex(q) complex( real(q), imag(q) ) 112 | % Q = ComplexMatrix(q) convert quaternions into complex 2x2 matrices 113 | % qc = conj(q) quaternion conjugate, qc.e = 114 | % [q.e(1);-q.e(2);-q.e(3);-q.e(4)] 115 | % qt = ctranspose(q) qt = q'; quaternion conjugate transpose, 116 | % 2-D (or scalar) q only 117 | % qp = cumprod(q,dim) cumulative quaternion array product over 118 | % dimension dim 119 | % qs = cumsum(q,dim) cumulative quaternion array sum over dimension dim 120 | % qd = diff(q,ord,dim) quaternion array difference, order ord, over 121 | % dimension dim 122 | % ans = display(q) 'q = ( e(1) ) + i( e(2) ) + j( e(3) ) + k( e(4) )' 123 | % d = dot(q1,q2) quaternion element dot product, d = dot(q1.e,q2.e) 124 | % d = double(q) d = q.e; if size(q) == [s1,s2,...], size(d) == 125 | % [4,s1,s2,...] 126 | % l = eq(q1,q2) quaternion equality, l = all( q1.e == q2.e ) 127 | % l = equiv(q1,q2,tol) quaternion rotational equivalence, within 128 | % tolerance tol, l = (q1 == q2) | (q1 == -q2) 129 | % qe = exp(q) quaternion exponential, v = q.e(2:4), qe.e = 130 | % exp(q.e(1))*[cos(|v|);v.*sin(|v|)./|v|] 131 | % ei = imag(q) imaginary e(2) components 132 | % qi = interp1(t,q,ti,method) interpolate quaternion array 133 | % qi = inverse(q) quaternion inverse, qi = conj(q)./norm(q).^2, 134 | % q .* qi = qi .* q = 1 for q ~= 0 135 | % l = isequal(q1,q2,...) true if equal sizes and values 136 | % l = isequaln(q1,q2,...) true if equal including NaNs 137 | % l = isequalwithequalnans(q1,q2,...) true if equal including NaNs 138 | % l = isfinite(q) true if all( isfinite( q.e )) 139 | % l = isinf(q) true if any( isinf( q.e )) 140 | % l = isnan(q) true if any( isnan( q.e )) 141 | % ej = jmag(q) e(3) components 142 | % ek = kmag(q) e(4) components 143 | % q3 = ldivide(q1,q2) quaternion left division, q3 = q1 \. q2 = 144 | % inverse(q1) *. q2 145 | % ql = log(q) quaternion logarithm, v = q.e(2:4), ql.e = 146 | % [log(|q|);v.*acos(q.e(1)./|q|)./|v|] 147 | % q3 = minus(q1,q2) quaternion subtraction, q3 = q1 - q2 148 | % q3 = mldivide(q1,q2) left division only defined for scalar q1 149 | % qp = mpower(q,p) quaternion matrix power, qp = q^p, p scalar 150 | % integer >= 0, q square quaternion matrix 151 | % q3 = mrdivide(q1,q2) right division only defined for scalar q2 152 | % q3 = mtimes(q1,q2) 2-D matrix quaternion multiplication, q3 = q1 * q2 153 | % l = ne(q1,q2) quaternion inequality, l = ~all( q1.e == q2.e ) 154 | % n = norm(q) quaternion norm, n = sqrt( sum( q.e.^2 )) 155 | % [q,n] = normalize(q) make quaternion norm == 1, unless q == 0, 156 | % n = matrix of previous norms 157 | % q3 = plus(q1,q2) quaternion addition, q3 = q1 + q2 158 | % qp = power(q,p) quaternion power, qp = q.^p 159 | % qp = prod(q,dim) quaternion array product over dimension dim 160 | % qp = product(q1,q2) quaternion product of scalar quaternions, 161 | % qp = q1 .* q2, noncommutative 162 | % q3 = rdivide(q1,q2) quaternion right division, q3 = q1 ./ q2 = 163 | % q1 .* inverse(q2) 164 | % er = real(q) real e(1) components 165 | % qs = slerp(q0,q1,t) quaternion spherical linear interpolation 166 | % qr = sqrt(q) qr = q.^0.5, square root 167 | % qs = sum(q,dim) quaternion array sum over dimension dim 168 | % q3 = times(q1,q2) matrix component quaternion multiplication, 169 | % q3 = q1 .* q2, noncommutative 170 | % qm = uminus(q) quaternion negation, qm = -q 171 | % qp = uplus(q) quaternion unitary plus, qp = +q 172 | % ev = vector(q) vector e(2:4) components 173 | % 174 | % Author: 175 | % Mark Tincknell, MIT LL, 29 July 2011, revised 25 June 2015 176 | 177 | properties (SetAccess = protected) 178 | e = zeros(4,1); 179 | end % properties 180 | 181 | % Array constructors 182 | methods 183 | function q = quaternion( varargin ) % (constructor) 184 | perm = []; 185 | sqz = false; 186 | switch nargin 187 | 188 | case 0 % nargin == 0 189 | q.e = zeros(4,1); 190 | return; 191 | 192 | case 1 % nargin == 1 193 | siz = size( varargin{1} ); 194 | nel = prod( siz ); 195 | if nel == 0 196 | q = quaternion.empty; 197 | return; 198 | elseif isa( varargin{1}, 'quaternion' ) 199 | q = varargin{1}; 200 | return; 201 | elseif (nel == 1) || ~isreal( varargin{1}(:) ) 202 | for iel = nel : -1 : 1 203 | q(iel).e = chop( [real(varargin{1}(iel)); ... 204 | imag(varargin{1}(iel)); ... 205 | 0; ... 206 | 0] ); 207 | end 208 | q = reshape( q, siz ); 209 | return; 210 | end 211 | [arg4, dim4, perm4] = finddim( varargin{1}, 4 ); 212 | if dim4 > 0 213 | siz(dim4) = 1; 214 | nel = prod( siz ); 215 | if dim4 > 1 216 | perm = perm4; 217 | else 218 | sqz = true; 219 | end 220 | for iel = nel : -1 : 1 221 | q(iel).e = chop( arg4(:,iel) ); 222 | end 223 | else 224 | [arg3, dim3, perm3] = finddim( varargin{1}, 3 ); 225 | if dim3 > 0 226 | siz(dim3) = 1; 227 | nel = prod( siz ); 228 | if dim3 > 1 229 | perm = perm3; 230 | else 231 | sqz = true; 232 | end 233 | for iel = nel : -1 : 1 234 | q(iel).e = chop( [0; arg3(:,iel)] ); 235 | end 236 | else 237 | error( 'quaternion:constructor:nargin1', ... 238 | 'Invalid input' ); 239 | end 240 | end 241 | 242 | case 2 % nargin == 2 243 | % real-imaginary only (no j or k) inputs 244 | na = cellfun( 'prodofsize', varargin ); 245 | [nel, jel] = max( na ); 246 | if ~all( (na == 1) | (na == nel) ) 247 | error( 'quaternion:constructor:nargin2', ... 248 | 'All inputs must be singletons or have the same number of elements' ); 249 | end 250 | siz = size( varargin{jel} ); 251 | for iel = nel : -1 : 1 252 | q(iel).e = chop( [varargin{1}(min(iel,na(1))); ... 253 | varargin{2}(min(iel,na(2))); ... 254 | 0; 255 | 0] ); 256 | end 257 | 258 | case 3 % nargin == 3 259 | % vector inputs (no real, only i, j, k) 260 | na = cellfun( 'prodofsize', varargin ); 261 | [nel, jel] = max( na ); 262 | if ~all( (na == 1) | (na == nel) ) 263 | error( 'quaternion:constructor:nargin3', ... 264 | 'All inputs must be singletons or have the same number of elements' ); 265 | end 266 | siz = size( varargin{jel} ); 267 | for iel = nel : -1 : 1 268 | q(iel).e = chop( [0; ... 269 | varargin{1}(min(iel,na(1))); ... 270 | varargin{2}(min(iel,na(2))); ... 271 | varargin{3}(min(iel,na(3)))] ); 272 | end 273 | 274 | otherwise % nargin >= 4 275 | na = cellfun( 'prodofsize', varargin ); 276 | [nel, jel] = max( na ); 277 | if ~all( (na == 1) | (na == nel) ) 278 | error( 'quaternion:constructor:nargin4', ... 279 | 'All inputs must be singletons or have the same number of elements' ); 280 | end 281 | siz = size( varargin{jel} ); 282 | for iel = nel : -1 : 1 283 | q(iel).e = chop( [varargin{1}(min(iel,na(1))); ... 284 | varargin{2}(min(iel,na(2))); ... 285 | varargin{3}(min(iel,na(3))); ... 286 | varargin{4}(min(iel,na(4)))] ); 287 | end 288 | end % switch nargin 289 | 290 | if nel == 0 291 | q = quaternion.empty; 292 | end 293 | q = reshape( q, siz ); 294 | if ~isempty( perm ) 295 | q = ipermute( q, perm ); 296 | end 297 | if sqz 298 | q = squeeze( q ); 299 | end 300 | end % quaternion (constructor) 301 | 302 | % Ordinary methods 303 | function n = abs( q ) 304 | n = q.norm; 305 | end % abs 306 | 307 | function q3 = bsxfun( func, q1, q2 ) 308 | % function q3 = bsxfun( func, q1, q2 ) 309 | % Binary Singleton Expansion for quaternion arrays. Apply the element by 310 | % element binary operation specified by the function handle func to arrays 311 | % q1 and q2. All dimensions of q1 and q2 must either agree or be length 1. 312 | % Inputs: 313 | % func function handle (e.g. @plus) of quaternion function or operator 314 | % q1(n1) quaternion array 315 | % q2(n2) quaternion array 316 | % Output: 317 | % q3(n3) quaternion array of function or operator outputs 318 | % size(q3) = max( size(q1), size(q2) ) 319 | if ~isa( q1, 'quaternion' ) 320 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 321 | end 322 | if ~isa( q2, 'quaternion' ) 323 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 324 | end 325 | s1 = size( q1 ); 326 | s2 = size( q2 ); 327 | nd1 = length( s1 ); 328 | nd2 = length( s2 ); 329 | s1 = [s1, ones(1,nd2-nd1)]; 330 | s2 = [s2, ones(1,nd1-nd2)]; 331 | if ~all( (s1 == s2) | (s1 == 1) | (s2 == 1) ) 332 | error( 'quaternion:bsxfun:notconformable', ... 333 | 'Non-singleton dimensions of q1 and q2 must match each other' ); 334 | end 335 | c1 = num2cell( s1 ); 336 | c2 = num2cell( s2 ); 337 | s3 = max( s1, s2 ); 338 | nd3 = length( s3 ); 339 | n3 = prod( s3 ); 340 | q3 = quaternion.nan( s3 ); 341 | for i3 = 1 : n3 342 | [ix3{1:nd3}] = ind2sub( s3, i3 ); 343 | ix1 = cellfun( @min, ix3, c1, 'UniformOutput', false ); 344 | ix2 = cellfun( @min, ix3, c2, 'UniformOutput', false ); 345 | q3(i3) = func( q1(ix1{:}), q2(ix2{:}) ); 346 | end 347 | end % bsxfun 348 | 349 | function c = complex( q ) 350 | c = complex( real( q ), imag( q )); 351 | end % complex 352 | 353 | function qc = conj( q ) 354 | d = double( q ); 355 | qc = reshape( quaternion( d(1,:), -d(2,:), -d(3,:), -d(4,:) ), ... 356 | size( q )); 357 | end % conj 358 | 359 | function qt = ctranspose( q ) 360 | qt = transpose( q.conj ); 361 | end % ctranspose 362 | 363 | function qp = cumprod( q, dim ) 364 | % function qp = cumprod( q, dim ) 365 | % cumulative quaternion array product, dim defaults to first dimension of 366 | % length > 1 367 | if isempty( q ) 368 | qp = q; 369 | return; 370 | end 371 | if (nargin < 2) || isempty( dim ) 372 | [q, dim, perm] = finddim( q, -2 ); 373 | elseif dim > 1 374 | ndm = ndims( q ); 375 | perm = [ dim : ndm, 1 : dim-1 ]; 376 | q = permute( q, perm ); 377 | end 378 | qp = q; 379 | for is = 2 : size(q,1) 380 | qp(is,:) = qp(is-1,:) .* q(is,:); 381 | end 382 | if dim > 1 383 | qp = ipermute( qp, perm ); 384 | end 385 | end % cumprod 386 | 387 | function qs = cumsum( q, dim ) 388 | % function qs = cumsum( q, dim ) 389 | % cumulative quaternion array sum, dim defaults to first dimension of 390 | % length > 1 391 | if isempty( q ) 392 | qs = q; 393 | return; 394 | end 395 | if (nargin < 2) || isempty( dim ) 396 | [q, dim, perm] = finddim( q, -2 ); 397 | elseif dim > 1 398 | ndm = ndims( q ); 399 | perm = [ dim : ndm, 1 : dim-1 ]; 400 | q = permute( q, perm ); 401 | end 402 | qs = q; 403 | for is = 2 : size(q,1) 404 | qs(is,:) = qs(is-1,:) + q(is,:); 405 | end 406 | if dim > 1 407 | qs = ipermute( qs, perm ); 408 | end 409 | end % cumsum 410 | 411 | function qd = diff( q, ord, dim ) 412 | % function qd = diff( q, ord, dim ) 413 | % quaternion array difference, ord is the order of difference (default = 1) 414 | % dim defaults to first dimension of length > 1 415 | if isempty( q ) 416 | qd = q; 417 | return; 418 | end 419 | if (nargin < 2) || isempty( ord ) 420 | ord = 1; 421 | end 422 | if ord <= 0 423 | qd = q; 424 | return; 425 | end 426 | if (nargin < 3) || isempty( dim ) 427 | [q, dim, perm] = finddim( q, -2 ); 428 | elseif dim > 1 429 | ndm = ndims( q ); 430 | perm = [ dim : ndm, 1 : dim-1 ]; 431 | q = permute( q, perm ); 432 | end 433 | siz = size( q ); 434 | if siz(1) <= 1 435 | qd = quaternion.empty; 436 | return; 437 | end 438 | qd = quaternion.zeros( [(siz(1)-1), siz(2:end)] ); 439 | for is = 1 : siz(1)-1 440 | qd(is,:) = q(is+1,:) - q(is,:); 441 | end 442 | ord = ord - 1; 443 | if ord > 0 444 | qd = diff( qd, ord, 1 ); 445 | end 446 | if dim > 1 447 | qd = ipermute( qd, perm ); 448 | end 449 | end % diff 450 | 451 | function display( q ) %#ok 452 | if ~isequal( get(0,'FormatSpacing'), 'compact' ) 453 | disp(' '); 454 | end 455 | if isempty( q ) 456 | fprintf( '%s \t= ([]) + i([]) + j([]) + k([])\n', inputname(1) ) 457 | return; 458 | end 459 | siz = size( q ); 460 | nel = [1 cumprod( siz )]; 461 | ndm = length( siz ); 462 | for iel = 1 : nel(end) 463 | if nel(end) == 1 464 | sub = ''; 465 | else 466 | sub = ')'; 467 | jel = iel - 1; 468 | for idm = ndm : -1 : 1 469 | idx = floor( jel / nel(idm) ) + 1; 470 | sub = [',' int2str(idx) sub]; %#ok 471 | jel = rem( jel, nel(idm) ); 472 | end 473 | sub(1) = '('; 474 | end 475 | fprintf( '%s%s \t= (%-12.5g) + i(%-12.5g) + j(%-12.5g) + k(%-12.5g)\n', ... 476 | inputname(1), sub, q(iel).e ) 477 | end 478 | end % display 479 | 480 | function d = dot( q1, q2 ) 481 | % function d = dot( q1, q2 ) 482 | % quaternion element dot product: d = dot( q1.e, q2.e ), using binary 483 | % singleton expansion of quaternion arrays 484 | % dn = dot( q1, q2 )/( norm(q1) * norm(q2) ) is the cosine of the angle in 485 | % 4D space between 4D vectors q1.e and q2.e 486 | d = squeeze( sum( bsxfun( @times, double( q1 ), double( q2 )), 1 )); 487 | end % dot 488 | 489 | function d = double( q ) 490 | siz = size( q ); 491 | d = reshape( [q.e], [4 siz] ); 492 | d = chop( d ); 493 | end % double 494 | 495 | function l = eq( q1, q2 ) 496 | if ~isa( q1, 'quaternion' ) 497 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 498 | end 499 | if ~isa( q2, 'quaternion' ) 500 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 501 | end 502 | si1 = size( q1 ); 503 | si2 = size( q2 ); 504 | ne1 = prod( si1 ); 505 | ne2 = prod( si2 ); 506 | if (ne1 == 0) || (ne2 == 0) 507 | l = logical([]); 508 | return; 509 | elseif ne1 == 1 510 | siz = si2; 511 | elseif ne2 == 1 512 | siz = si1; 513 | elseif isequal( si1, si2 ) 514 | siz = si1; 515 | else 516 | error( 'quaternion:eq:baddims', ... 517 | 'Matrix dimensions must agree' ); 518 | end 519 | l = bsxfun( @eq, [q1.e], [q2.e] ); 520 | l = reshape( all( l, 1 ), siz ); 521 | end % eq 522 | 523 | function l = equiv( q1, q2, tol ) 524 | % function l = equiv( q1, q2, tol ) 525 | % quaternion rotational equivalence, within tolerance tol, 526 | % l = (q1 == q2) | (q1 == -q2) 527 | % optional argument tol (default = eps) sets tolerance for difference 528 | % from exact equality 529 | if ~isa( q1, 'quaternion' ) 530 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 531 | end 532 | if ~isa( q2, 'quaternion' ) 533 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 534 | end 535 | if (nargin < 3) || isempty( tol ) 536 | tol = eps; 537 | end 538 | si1 = size( q1 ); 539 | si2 = size( q2 ); 540 | ne1 = prod( si1 ); 541 | ne2 = prod( si2 ); 542 | if (ne1 == 0) || (ne2 == 0) 543 | l = logical([]); 544 | return; 545 | elseif ne1 == 1 546 | siz = si2; 547 | elseif ne2 == 1 548 | siz = si1; 549 | elseif isequal( si1, si2 ) 550 | siz = si1; 551 | else 552 | error( 'quaternion:equiv:baddims', ... 553 | 'Matrix dimensions must agree' ); 554 | end 555 | dm = chop( bsxfun( @minus, [q1.e], [q2.e] ), tol ); 556 | dp = chop( bsxfun( @plus, [q1.e], [q2.e] ), tol ); 557 | l = all( (dm == 0) | (dp == 0), 1 ); 558 | l = reshape( l, siz ); 559 | end % equiv 560 | 561 | function qe = exp( q ) 562 | % function qe = exp( q ) 563 | % quaternion exponential, v = q.e(2:4), 564 | % qe.e = exp(q.e(1))*[cos(|v|);v.*sin(|v|)./|v|] 565 | d = double( q ); 566 | siz = size( d ); 567 | od = ones( 1, ndims( q )); 568 | vn = reshape( sqrt( sum( d(2:4,:).^2, 1 )), [1 siz(2:end)] ); 569 | cv = cos( vn ); 570 | sv = sin( vn ); 571 | n0 = vn ~= 0; 572 | sv(n0) = sv(n0) ./ vn(n0); 573 | sv = repmat( sv, [3, od] ); 574 | ex = repmat( reshape( exp( d(1,:) ), [1 siz(2:end)] ), [4, od] ); 575 | de = ex .* [ cv; sv .* reshape( d(2:4,:), [3 siz(2:end)] )]; 576 | qe = reshape( quaternion( de(1,:), de(2,:), de(3,:), de(4,:) ), ... 577 | size( q )); 578 | end % exp 579 | 580 | function ei = imag( q ) 581 | siz = size( q ); 582 | d = double( q ); 583 | ei = reshape( d(2,:), siz ); 584 | end % imag 585 | 586 | function qi = interp1( varargin ) 587 | % function qi = interp1( t, q, ti, method ) or 588 | % qi = q.interp1( t, ti, method ) or 589 | % qi = interp1( q, ti, method ) 590 | % Interpolate quaternion array. If q are orientation quaternions (i.e. 591 | % normalized to 1), then -q is equivalent to q, and the sign of q to use as 592 | % the second knot of the interpolation is chosen by which ever is closer to 593 | % the first knot. Extrapolation (i.e. ti < min(t) or ti > max(t)) gives 594 | % qi = quaternion.nan. 595 | % Inputs: 596 | % t(nt) array of ordinates (e.g. times); if t is not provided t=1:nt 597 | % q(nt,nq) quaternion array 598 | % ti(ni) array of query (interpolation) points, t(1) <= ti <= t(end) 599 | % method [OPTIONAL] 'slerp' or 'linear'; default = 'slerp' 600 | % Output: 601 | % qi(ni,nq) interpolated quaternion array 602 | nna = nnz( ~cellfun( @ischar, varargin )); 603 | im = 4; 604 | if isa( varargin{1}, 'quaternion' ) 605 | q = varargin{1}; 606 | siq = size( q ); 607 | if nna == 2 608 | if size( q, 1 ) == 1 609 | t = (1 : siq(2)).'; 610 | else 611 | t = (1 : siq(1)).'; 612 | end 613 | ti = varargin{2}(:); 614 | im = 3; 615 | elseif isempty( varargin{2} ) 616 | if size( q, 1 ) == 1 617 | t = (1 : siq(2)).'; 618 | else 619 | t = (1 : siq(1)).'; 620 | end 621 | ti = varargin{3}(:); 622 | else 623 | t = varargin{2}(:); 624 | ti = varargin{3}(:); 625 | end 626 | elseif isa( varargin{2}, 'quaternion' ) 627 | t = varargin{1}(:); 628 | q = varargin{2}; 629 | ti = varargin{3}(:); 630 | siq = size( q ); 631 | else 632 | error( 'quaternion:interp1:notq', ... 633 | 'Input q must be a quaterion' ); 634 | end 635 | neq = prod( siq ); 636 | if neq == 0 637 | qi = quaternion.empty; 638 | return; 639 | end 640 | nt = numel( t ); 641 | if siq(1) == nt 642 | dim = 1; 643 | else 644 | [q, dim, perm] = finddim( q, nt ); 645 | if dim == 0 646 | error( 'quaternion:interp1:badt', ... 647 | 'q must have a dimension the same size as t' ); 648 | end 649 | end 650 | iNf = interp1( t, (1:nt).', ti ); 651 | iN = max( 1, min( nt-1, floor( iNf ))); 652 | jN = max( 2, min( nt, ceil( iNf ))); 653 | iNm = repmat( iNf - iN, [1, neq / nt] ); 654 | % If q are orientation quaternions (i.e. all normalized to 1), then -q 655 | % represents the same rotation. Pick the sign of +/-q that has the closest 656 | % dot product to use as the second knot of the interpolation. 657 | qj = q(jN,:); 658 | if all( abs( norm( q(:) ) - 1 ) <= eps(16) ) 659 | qd = dot( q(iN,:), qj ); 660 | lq = qd < -qd; 661 | qj(lq) = -qj(lq); 662 | end 663 | if (length( varargin ) >= im) && ... 664 | (strncmpi( 'linear', varargin{im}, length( varargin{im} ))) 665 | qi = (1 - iNm) .* q(iN,:) + iNm .* qj; 666 | else 667 | qi = slerp( q(iN,:), qj, iNm ); 668 | end 669 | if length( siq ) > 2 670 | sin = siq; 671 | sin(dim) = numel( ti ); 672 | sin = circshift( sin, [0, 1-dim] ); 673 | qi = reshape( qi, sin ); 674 | end 675 | if dim > 1 676 | qi = ipermute( qi, perm ); 677 | end 678 | end % interp1 679 | 680 | function qi = inverse( q ) 681 | % function qi = inverse( q ) 682 | % quaternion inverse, qi = conj(q)/norm(q)^2, q*qi = qi*q = 1 for q ~= 0 683 | if isempty( q ) 684 | qi = q; 685 | return; 686 | end 687 | d = double( q ); 688 | d(2:4,:) = -d(2:4,:); 689 | n2 = repmat( sum( d.^2, 1 ), [4, ones(1,ndims(d)-1 )] ); 690 | ne0 = n2 ~= 0; 691 | di = Inf( size( d )); 692 | di(ne0) = d(ne0) ./ n2(ne0); 693 | qi = reshape( quaternion( di(1,:), di(2,:), di(3,:), di(4,:) ), ... 694 | size( q )); 695 | end % inverse 696 | 697 | function l = isequal( q1, varargin ) 698 | % function l = isequal( q1, q2, ... ) 699 | nar = numel( varargin ); 700 | if nar == 0 701 | error( 'quaternion:isequal:noargs', ... 702 | 'Not enough input arguments' ); 703 | end 704 | l = false; 705 | if ~isa( q1, 'quaternion' ) 706 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 707 | end 708 | si1 = size( q1 ); 709 | for iar = 1 : nar 710 | si2 = size( varargin{iar} ); 711 | if (length( si1 ) ~= length( si2 )) || ... 712 | ~all( si1 == si2 ) 713 | return; 714 | else 715 | if ~isa( varargin{iar}, 'quaternion' ) 716 | q2 = quaternion( ... 717 | real(varargin{iar}), imag(varargin{iar}), 0, 0 ); 718 | else 719 | q2 = varargin{iar}; 720 | end 721 | if ~isequal( [q1.e], [q2.e] ) 722 | return; 723 | end 724 | end 725 | end 726 | l = true; 727 | end % isequal 728 | 729 | function l = isequaln( q1, varargin ) 730 | % function l = isequaln( q1, q2, ... ) 731 | nar = numel( varargin ); 732 | if nar == 0 733 | error( 'quaternion:isequaln:noargs', ... 734 | 'Not enough input arguments' ); 735 | end 736 | l = false; 737 | if ~isa( q1, 'quaternion' ) 738 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 739 | end 740 | si1 = size( q1 ); 741 | for iar = 1 : nar 742 | si2 = size( varargin{iar} ); 743 | if (length( si1 ) ~= length( si2 )) || ... 744 | ~all( si1 == si2 ) 745 | return; 746 | else 747 | if ~isa( varargin{iar}, 'quaternion' ) 748 | q2 = quaternion( ... 749 | real(varargin{iar}), imag(varargin{iar}), 0, 0 ); 750 | else 751 | q2 = varargin{iar}; 752 | end 753 | if ~isequaln( [q1.e], [q2.e] ) 754 | return; 755 | end 756 | end 757 | end 758 | l = true; 759 | end % isequaln 760 | 761 | function l = isequalwithequalnans( q1, varargin ) 762 | % function l = isequalwithequalnans( q1, q2, ... ) 763 | nar = numel( varargin ); 764 | if nar == 0 765 | error( 'quaternion:isequalwithequalnans:noargs', ... 766 | 'Not enough input arguments' ); 767 | end 768 | l = false; 769 | if ~isa( q1, 'quaternion' ) 770 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 771 | end 772 | si1 = size( q1 ); 773 | for iar = 1 : nar 774 | si2 = size( varargin{iar} ); 775 | if (length( si1 ) ~= length( si2 )) || ... 776 | ~all( si1 == si2 ) 777 | return; 778 | else 779 | if ~isa( varargin{iar}, 'quaternion' ) 780 | q2 = quaternion( ... 781 | real(varargin{iar}), imag(varargin{iar}), 0, 0 ); 782 | else 783 | q2 = varargin{iar}; 784 | end 785 | if ~isequalwithequalnans( [q1.e], [q2.e] ) %#ok 786 | return; 787 | end 788 | end 789 | end 790 | l = true; 791 | end % isequalwithequalnans 792 | 793 | function l = isfinite( q ) 794 | % function l = isfinite( q ), l = all( isfinite( q.e )) 795 | d = [q.e]; 796 | l = reshape( all( isfinite( d ), 1 ), size( q )); 797 | end % isfinite 798 | 799 | function l = isinf( q ) 800 | % function l = isinf( q ), l = any( isinf( q.e )) 801 | d = [q.e]; 802 | l = reshape( any( isinf( d ), 1 ), size( q )); 803 | end % isinf 804 | 805 | function l = isnan( q ) 806 | % function l = isnan( q ), l = any( isnan( q.e )) 807 | d = [q.e]; 808 | l = reshape( any( isnan( d ), 1 ), size( q )); 809 | end % isnan 810 | 811 | function ej = jmag( q ) 812 | siz = size( q ); 813 | d = double( q ); 814 | ej = reshape( d(3,:), siz ); 815 | end % jmag 816 | 817 | function ek = kmag( q ) 818 | siz = size( q ); 819 | d = double( q ); 820 | ek = reshape( d(4,:), siz ); 821 | end % kmag 822 | 823 | function q3 = ldivide( q1, q2 ) 824 | if ~isa( q1, 'quaternion' ) 825 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 826 | end 827 | if ~isa( q2, 'quaternion' ) 828 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 829 | end 830 | si1 = size( q1 ); 831 | si2 = size( q2 ); 832 | ne1 = prod( si1 ); 833 | ne2 = prod( si2 ); 834 | if (ne1 == 0) || (ne2 == 0) 835 | q3 = quaternion.empty; 836 | return; 837 | elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1) 838 | error( 'quaternion:ldivide:baddims', ... 839 | 'Matrix dimensions must agree' ); 840 | end 841 | for iel = max( ne1, ne2 ) : -1 : 1 842 | q3(iel) = product( q1(min(iel,ne1)).inverse, ... 843 | q2(min(iel,ne2)) ); 844 | end 845 | if ne2 > ne1 846 | q3 = reshape( q3, si2 ); 847 | else 848 | q3 = reshape( q3, si1 ); 849 | end 850 | end % ldivide 851 | 852 | function ql = log( q ) 853 | % function ql = log( q ) 854 | % quaternion logarithm, v = q.e(2:4), ql.e = [log(|q|);v.*acos(q.e(1)./|q|)./|v|] 855 | % logarithm of negative real quaternions is ql.e = [log(|q|);pi;0;0] 856 | d = double( q ); 857 | d2 = d.^2; 858 | siz = size( d ); 859 | od = ones( 1, ndims( q )); 860 | [vn,qn] = deal( zeros( [1 siz(2:end)] )); 861 | vn(:) = sqrt( sum( d2(2:4,:), 1 )); 862 | qn(:) = sqrt( sum( d2(1:4,:), 1 )); 863 | lq = log( qn ); 864 | d1 = reshape( d(1,:), [1 siz(2:end)] ); 865 | nq = qn ~= 0; 866 | d1(nq) = d1(nq) ./ qn(nq); 867 | ac = acos( d1 ); 868 | nv = vn ~= 0; 869 | ac(nv) = ac(nv) ./ vn(nv); 870 | ac = reshape( repmat( ac, [3, od] ), 3, [] ); 871 | va = reshape( d(2:4,:) .* ac, [3 siz(2:end)] ); 872 | nn = (d1 < 0) & (vn == 0); 873 | va(1,nn)= pi; 874 | dl = [ lq; va ]; 875 | ql = reshape( quaternion( dl(1,:), dl(2,:), dl(3,:), dl(4,:) ), ... 876 | size( q )); 877 | end % log 878 | 879 | function q3 = minus( q1, q2 ) 880 | if ~isa( q1, 'quaternion' ) 881 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 882 | end 883 | if ~isa( q2, 'quaternion' ) 884 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 885 | end 886 | si1 = size( q1 ); 887 | si2 = size( q2 ); 888 | ne1 = prod( si1 ); 889 | ne2 = prod( si2 ); 890 | if (ne1 == 0) || (ne2 == 0) 891 | q3 = quaternion.empty; 892 | return; 893 | elseif ne1 == 1 894 | siz = si2; 895 | elseif ne2 == 1 896 | siz = si1; 897 | elseif isequal( si1, si2 ) 898 | siz = si1; 899 | else 900 | error( 'quaternion:minus:baddims', ... 901 | 'Matrix dimensions must agree' ); 902 | end 903 | d3 = bsxfun( @minus, [q1.e], [q2.e] ); 904 | q3 = quaternion( d3(1,:), d3(2,:), d3(3,:), d3(4,:) ); 905 | q3 = reshape( q3, siz ); 906 | end % minus 907 | 908 | function q3 = mldivide( q1, q2 ) 909 | % function q3 = mldivide( q1, q2 ), left division only defined for scalar q1 910 | if numel( q1 ) > 1 911 | error( 'quaternion:mldivide:undefined', ... 912 | 'Left matix division undefined for quaternion arrays' ); 913 | end 914 | q3 = ldivide( q1, q2 ); 915 | end % mldivide 916 | 917 | function qp = mpower( q, p ) 918 | % function qp = mpower( q, p ), quaternion matrix power 919 | siq = size( q ); 920 | neq = prod( siq ); 921 | nep = numel( p ); 922 | if neq == 1 923 | qp = power( q, p ); 924 | return; 925 | elseif isa( p, 'quaternion' ) 926 | error( 'quaternion:mpower:undefined', ... 927 | 'Quaternion as matrix exponent is not defined' ); 928 | end 929 | if (neq == 0) || (nep == 0) 930 | qp = quaternion.empty; 931 | return; 932 | elseif (nep > 1) || (mod( p, 1 ) ~= 0) || (p < 0) || ... 933 | (numel( siq ) > 2) || (siq(1) ~= siq(2)) 934 | error( 'quaternion:mpower:notconformable', ... 935 | 'Inputs must be a scalar non-negative integer power and a square quaternion matrix' ); 936 | elseif p == 0 937 | qp = quaternion.eye( siq(1) ); 938 | return; 939 | end 940 | qp = q; 941 | for ip = 2 : p 942 | qp = qp * q; 943 | end 944 | end % mpower 945 | 946 | function q3 = mrdivide( q1, q2 ) 947 | % function q3 = mrdivide( q1, q2 ), right division only defined for scalar q2 948 | if numel( q2 ) > 1 949 | error( 'quaternion:mrdivide:undefined', ... 950 | 'Right matix division undefined for quaternion arrays' ); 951 | end 952 | q3 = rdivide( q1, q2 ); 953 | end % mrdivide 954 | 955 | function q3 = mtimes( q1, q2 ) 956 | % function q3 = mtimes( q1, q2 ) 957 | % q3 = matrix quaternion product of 2-D conformable quaternion matrices q1 958 | % and q2 959 | if ~isa( q1, 'quaternion' ) 960 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 961 | end 962 | if ~isa( q2, 'quaternion' ) 963 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 964 | end 965 | si1 = size( q1 ); 966 | si2 = size( q2 ); 967 | ne1 = prod( si1 ); 968 | ne2 = prod( si2 ); 969 | if (ne1 == 1) || (ne2 == 1) 970 | q3 = times( q1, q2 ); 971 | return; 972 | end 973 | if (length( si1 ) ~= 2) || (length( si2 ) ~= 2) 974 | error( 'quaternion:mtimes:not2D', ... 975 | 'Input arguments must be 2-D' ); 976 | end 977 | if si1(2) ~= si2(1) 978 | error( 'quaternion:mtimes:notconformable', ... 979 | 'Inner matrix dimensions must agree' ); 980 | end 981 | q3 = repmat( quaternion, [si1(1) si2(2)] ); 982 | for i1 = 1 : si1(1) 983 | for i2 = 1 : si2(2) 984 | for i3 = 1 : si1(2) 985 | q3(i1,i2) = q3(i1,i2) + product( q1(i1,i3), q2(i3,i2) ); 986 | end 987 | end 988 | end 989 | end % mtimes 990 | 991 | function l = ne( q1, q2 ) 992 | l = ~eq( q1, q2 ); 993 | end % ne 994 | 995 | function n = norm( q ) 996 | n = shiftdim( sqrt( sum( double( q ).^2, 1 )), 1 ); 997 | end % norm 998 | 999 | function [q, n] = normalize( q ) 1000 | % function [q, n] = normalize( q ) 1001 | % q = quaternions with norm == 1 (unless q == 0), n = former norms 1002 | siz = size( q ); 1003 | nel = prod( siz ); 1004 | if nel == 0 1005 | if nargout > 1 1006 | n = zeros( siz ); 1007 | end 1008 | return; 1009 | end 1010 | d = double( q ); 1011 | n = sqrt( sum( d.^2, 1 )); 1012 | if all( n(:) == 1 ) 1013 | if nargout > 1 1014 | n = shiftdim( n, 1 ); 1015 | end 1016 | return; 1017 | end 1018 | n4 = repmat( n, [4, ones(1,ndims(n)-1)] ); 1019 | ne0 = (n4 ~= 0) & (n4 ~= 1); 1020 | d(ne0) = d(ne0) ./ n4(ne0); 1021 | q = reshape( quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ), siz ); 1022 | if nargout > 1 1023 | n = shiftdim( n, 1 ); 1024 | end 1025 | end % normalize 1026 | 1027 | function q3 = plus( q1, q2 ) 1028 | if ~isa( q1, 'quaternion' ) 1029 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 1030 | end 1031 | if ~isa( q2, 'quaternion' ) 1032 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 1033 | end 1034 | si1 = size( q1 ); 1035 | si2 = size( q2 ); 1036 | ne1 = prod( si1 ); 1037 | ne2 = prod( si2 ); 1038 | if (ne1 == 0) || (ne2 == 0) 1039 | q3 = quaternion.empty; 1040 | return; 1041 | elseif ne1 == 1 1042 | siz = si2; 1043 | elseif ne2 == 1 1044 | siz = si1; 1045 | elseif isequal( si1, si2 ) 1046 | siz = si1; 1047 | else 1048 | error( 'quaternion:plus:baddims', ... 1049 | 'Matrix dimensions must agree' ); 1050 | end 1051 | d3 = bsxfun( @plus, [q1.e], [q2.e] ); 1052 | q3 = quaternion( d3(1,:), d3(2,:), d3(3,:), d3(4,:) ); 1053 | q3 = reshape( q3, siz ); 1054 | end % plus 1055 | 1056 | function qp = power( q, p ) 1057 | % function qp = power( q, p ), quaternion power 1058 | siq = size( q ); 1059 | sip = size( p ); 1060 | neq = prod( siq ); 1061 | nep = prod( sip ); 1062 | if (neq == 0) || (nep == 0) 1063 | qp = quaternion.empty; 1064 | return; 1065 | elseif ~isequal( siq, sip ) && (neq ~= 1) && (nep ~= 1) 1066 | error( 'quaternion:power:baddims', ... 1067 | 'Matrix dimensions must agree' ); 1068 | end 1069 | qp = exp( p .* log( q )); 1070 | end % power 1071 | 1072 | function qp = prod( q, dim ) 1073 | % function qp = prod( q, dim ) 1074 | % quaternion array product over dimension dim 1075 | % dim defaults to first dimension of length > 1 1076 | if isempty( q ) 1077 | qp = q; 1078 | return; 1079 | end 1080 | if (nargin < 2) || isempty( dim ) 1081 | [q, dim, perm] = finddim( q, -2 ); 1082 | elseif dim > 1 1083 | ndm = ndims( q ); 1084 | perm = [ dim : ndm, 1 : dim-1 ]; 1085 | q = permute( q, perm ); 1086 | end 1087 | siz = size( q ); 1088 | qp = reshape( q(1,:), [1 siz(2:end)] ); 1089 | for is = 2 : siz(1) 1090 | qp(1,:) = qp(1,:) .* q(is,:); 1091 | end 1092 | if dim > 1 1093 | qp = ipermute( qp, perm ); 1094 | end 1095 | end % prod 1096 | 1097 | function q3 = product( q1, q2 ) 1098 | % function q3 = product( q1, q2 ) 1099 | % q3 = quaternion product of scalar quaternions q1 and q2 1100 | if ~isa( q1, 'quaternion' ) 1101 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 1102 | end 1103 | if ~isa( q2, 'quaternion' ) 1104 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 1105 | end 1106 | if (numel( q1 ) ~= 1) || (numel( q2 ) ~= 1) 1107 | error( 'quaternion:product:onlyscalars', ... 1108 | 'product not defined for arrays, use mtimes or times' ); 1109 | end 1110 | ee = q1.e * q2.e.'; 1111 | eo = [ee(1,1) - ee(2,2) - ee(3,3) - ee(4,4); ... 1112 | ee(1,2) + ee(2,1) + ee(3,4) - ee(4,3); ... 1113 | ee(1,3) - ee(2,4) + ee(3,1) + ee(4,2); ... 1114 | ee(1,4) + ee(2,3) - ee(3,2) + ee(4,1)]; 1115 | eo = chop( eo ); 1116 | q3 = quaternion( eo(1), eo(2), eo(3), eo(4) ); 1117 | end % product 1118 | 1119 | function q3 = rdivide( q1, q2 ) 1120 | if ~isa( q1, 'quaternion' ) 1121 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 1122 | end 1123 | if ~isa( q2, 'quaternion' ) 1124 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 1125 | end 1126 | si1 = size( q1 ); 1127 | si2 = size( q2 ); 1128 | ne1 = prod( si1 ); 1129 | ne2 = prod( si2 ); 1130 | if (ne1 == 0) || (ne2 == 0) 1131 | q3 = quaternion.empty; 1132 | return; 1133 | elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1) 1134 | error( 'quaternion:rdivide:baddims', ... 1135 | 'Matrix dimensions must agree' ); 1136 | end 1137 | for iel = max( ne1, ne2 ) : -1 : 1 1138 | q3(iel) = product( q1(min(iel,ne1)), ... 1139 | q2(min(iel,ne2)).inverse ); 1140 | end 1141 | if ne2 > ne1 1142 | q3 = reshape( q3, si2 ); 1143 | else 1144 | q3 = reshape( q3, si1 ); 1145 | end 1146 | end % rdivide 1147 | 1148 | function er = real( q ) 1149 | siz = size( q ); 1150 | d = double( q ); 1151 | er = reshape( d(1,:), siz ); 1152 | end % real 1153 | 1154 | function qs = slerp( q0, q1, t ) 1155 | % function qs = slerp( q0, q1, t ) 1156 | % quaternion spherical linear interpolation, qs = q0.*(q0.inverse.*q1).^t, 1157 | % default t = 0.5; see http://en.wikipedia.org/wiki/Slerp 1158 | if (nargin < 3) || isempty( t ) 1159 | t = 0.5; 1160 | end 1161 | qs = q0 .* (q0.inverse .* q1).^t; 1162 | end % slerp 1163 | 1164 | function qr = sqrt( q ) 1165 | qr = q.^0.5; 1166 | end % sqrt 1167 | 1168 | function qs = sum( q, dim ) 1169 | % function qs = sum( q, dim ) 1170 | % quaternion array sum over dimension dim 1171 | % dim defaults to first dimension of length > 1 1172 | if isempty( q ) 1173 | qs = q; 1174 | return; 1175 | end 1176 | if (nargin < 2) || isempty( dim ) 1177 | [q, dim, perm] = finddim( q, -2 ); 1178 | elseif dim > 1 1179 | ndm = ndims( q ); 1180 | perm = [ dim : ndm, 1 : dim-1 ]; 1181 | q = permute( q, perm ); 1182 | end 1183 | siz = size( q ); 1184 | qs = reshape( q(1,:), [1 siz(2:end)] ); 1185 | for is = 2 : siz(1) 1186 | qs(1,:) = qs(1,:) + q(is,:); 1187 | end 1188 | if dim > 1 1189 | qs = ipermute( qs, perm ); 1190 | end 1191 | end % sum 1192 | 1193 | function q3 = times( q1, q2 ) 1194 | if ~isa( q1, 'quaternion' ) 1195 | q1 = quaternion( real(q1), imag(q1), 0, 0 ); 1196 | end 1197 | if ~isa( q2, 'quaternion' ) 1198 | q2 = quaternion( real(q2), imag(q2), 0, 0 ); 1199 | end 1200 | si1 = size( q1 ); 1201 | si2 = size( q2 ); 1202 | ne1 = prod( si1 ); 1203 | ne2 = prod( si2 ); 1204 | if (ne1 == 0) || (ne2 == 0) 1205 | q3 = quaternion.empty; 1206 | return; 1207 | elseif ~isequal( si1, si2 ) && (ne1 ~= 1) && (ne2 ~= 1) 1208 | error( 'quaternion:times:baddims', ... 1209 | 'Matrix dimensions must agree' ); 1210 | end 1211 | for iel = max( ne1, ne2 ) : -1 : 1 1212 | q3(iel) = product( q1(min(iel,ne1)), q2(min(iel,ne2)) ); 1213 | end 1214 | if ne2 > ne1 1215 | q3 = reshape( q3, si2 ); 1216 | else 1217 | q3 = reshape( q3, si1 ); 1218 | end 1219 | end % times 1220 | 1221 | function qm = uminus( q ) 1222 | d = -double( q ); 1223 | qm = reshape( quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ), ... 1224 | size( q )); 1225 | end % uminus 1226 | 1227 | function qp = uplus( q ) 1228 | qp = q; 1229 | end % uplus 1230 | 1231 | function ev = vector( q ) 1232 | siz = size( q ); 1233 | d = double( q ); 1234 | ev = reshape( d(2:4,:), [3 siz] ); 1235 | end % vector 1236 | 1237 | function [angle, axis] = AngleAxis( q ) 1238 | % function [angle, axis] = AngleAxis( q ) or [angle, axis] = q.AngleAxis 1239 | % Construct angle-axis pairs equivalent to quaternion rotations 1240 | % Input: 1241 | % q quaternion array 1242 | % Outputs: 1243 | % angle rotation angles in radians, 0 <= angle <= 2*pi 1244 | % axis 3xN or Nx3 rotation axis unit vectors 1245 | % Note: angle and axis are constructed so at least 2 out of 3 elements of 1246 | % axis are >= 0. 1247 | siz = size( q ); 1248 | ndm = length( siz ); 1249 | [angle, s] = deal( zeros( siz )); 1250 | axis = zeros( [3 siz] ); 1251 | nel = prod( siz ); 1252 | if nel == 0 1253 | return; 1254 | end 1255 | [q, n] = normalize( q ); 1256 | d = double( q ); 1257 | neg = repmat( reshape( d(1,:) < 0, [1 siz] ), ... 1258 | [4, ones(1,ndm)] ); 1259 | d(neg) = -d(neg); 1260 | angle(1:end)= 2 * acos( d(1,:) ); 1261 | s(1:end) = sin( 0.5 * angle ); 1262 | angle(n==0) = 0; 1263 | s(s==0) = 1; 1264 | s3 = shiftdim( s, -1 ); 1265 | axis(1:end) = bsxfun( @rdivide, reshape( d(2:4,:), [3 siz] ), s3 ); 1266 | axis(1,(mod(angle,2*pi)==0)) = 1; 1267 | angle = chop( angle ); 1268 | axis = chop( axis ); 1269 | % Flip axis so at least 2 out of 3 elements are >= 0 1270 | flip = (sum( axis < 0, 1 ) > 1) | ... 1271 | ((sum( axis == 0, 1 ) == 2) & (any( axis < 0, 1 ) == 1)); 1272 | angle(flip) = 2 * pi - angle(flip); 1273 | flip = repmat( flip, [3, ones(1,ndm)] ); 1274 | axis(flip) = -axis(flip); 1275 | axis = squeeze( axis ); 1276 | end % AngleAxis 1277 | 1278 | function Q = ComplexMatrix( q ) 1279 | % function Q = ComplexMatrix( q ) or Q = q.ComplexMatrix 1280 | % Convert quaternions into special complex 2x2 matrices. Matrix algebra 1281 | % with Q (e.g. sum, difference, matrix product, matrix inverse) is 1282 | % equivalent to the same operation with quaternions. Quaternions can be 1283 | % created from 2x2 complex matrices with quaternion.complexmatrix(Q) 1284 | % (lower case). 1285 | % Input: 1286 | % q array of N quaternions, q.e = [A; B; C; D] 1287 | % Output: 1288 | % Q 2x2xN array of complex matrices 1289 | % Q = [ A + B*i, C - D*i; 1290 | % -C - D*i, A - B*i] 1291 | siz = size( q ); 1292 | d = double( q ); 1293 | Q = [complex( d(1,:), d(2,:)); 1294 | complex(-d(3,:),-d(4,:)); 1295 | complex( d(3,:),-d(4,:)); 1296 | complex( d(1,:),-d(2,:))]; 1297 | Q = squeeze( reshape( Q, [2, 2, siz] )); 1298 | end % ComplexMatrix 1299 | 1300 | function qd = Derivative( varargin ) 1301 | % function qd = Derivative( q, w ) or qd = q.Derivative( w ) 1302 | % Inputs: 1303 | % q quaternion array 1304 | % w 3xN or Nx3 element angle rate vectors in radians/s 1305 | % Output: 1306 | % qd quaternion derivatives, qd = 0.5 * q * quaternion(w) 1307 | if isa( varargin{1}, 'quaternion' ) 1308 | qd = 0.5 .* varargin{1} .* quaternion( varargin{2} ); 1309 | else 1310 | qd = 0.5 .* varargin{2} .* quaternion( varargin{1} ); 1311 | end 1312 | end % Derivative 1313 | 1314 | function angles = EulerAngles( varargin ) 1315 | % function angles = EulerAngles( q, axes ) or angles = q.EulerAngles( axes ) 1316 | % Construct Euler angle triplets equivalent to quaternion rotations 1317 | % Inputs: 1318 | % q quaternion array 1319 | % axes axes designation strings (e.g. '123' = xyz) or cell strings 1320 | % (e.g. {'123'}) 1321 | % Output: 1322 | % angles 3 element Euler Angle vectors in radians 1323 | ics = cellfun( @ischar, varargin ); 1324 | if any( ics ) 1325 | varargin{ics} = cellstr( varargin{ics} ); 1326 | else 1327 | ics = cellfun( @iscellstr, varargin ); 1328 | end 1329 | if ~any( ics ) 1330 | error( 'quaternion:EulerAngles:badaxes', ... 1331 | 'Must provide axes as a string (e.g. ''123'') or cell string (e.g. {''123''})' ); 1332 | end 1333 | siv = cellfun( @size, varargin, 'UniformOutput', false ); 1334 | axes = varargin{ics}; 1335 | six = siv{ics}; 1336 | nex = prod( six ); 1337 | q = varargin{~ics}; 1338 | siq = siv{~ics}; 1339 | neq = prod( siq ); 1340 | if neq == 1 1341 | siz = six; 1342 | nel = nex; 1343 | elseif nex == 1 1344 | siz = siq; 1345 | nel = neq; 1346 | elseif nex == neq 1347 | siz = siq; 1348 | nel = neq; 1349 | else 1350 | error( 'quaternion:EulerAngles:notconformable', ... 1351 | 'Must have compatible dimensions for quaternion and axes' ); 1352 | end 1353 | angles = zeros( [3 siz] ); 1354 | q = normalize( q ); 1355 | for jel = 1 : nel 1356 | iel = min( jel, neq ); 1357 | switch axes{min(jel,nex)} 1358 | case {'121', 'xyx', 'XYX', 'iji'} 1359 | angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(3)- ... 1360 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(4)+ ... 1361 | q(iel).e(3).*q(iel).e(1))); 1362 | angles(2,iel) = acos(q(iel).e(1).^2+q(iel).e(2).^2- ... 1363 | q(iel).e(3).^2-q(iel).e(4).^2); 1364 | angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(3)+ ... 1365 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(3).*q(iel).e(1)- ... 1366 | q(iel).e(2).*q(iel).e(4))); 1367 | case {'123', 'xyz', 'XYZ', 'ijk'} 1368 | angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)+ ... 1369 | q(iel).e(4).*q(iel).e(3)),(q(iel).e(1).^2- ... 1370 | q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2)); 1371 | angles(2,iel) = asin(2.*(q(iel).e(3).*q(iel).e(1)- ... 1372 | q(iel).e(2).*q(iel).e(4))); 1373 | angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(3)+ ... 1374 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(1).^2+ ... 1375 | q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2)); 1376 | case {'131', 'xzx', 'XZX', 'iki'} 1377 | angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(4)+ ... 1378 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(4).*q(iel).e(1)- ... 1379 | q(iel).e(2).*q(iel).e(3))); 1380 | angles(2,iel) = acos(q(iel).e(1).^2+q(iel).e(2).^2- ... 1381 | q(iel).e(3).^2-q(iel).e(4).^2); 1382 | angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(4)- ... 1383 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(3)+ ... 1384 | q(iel).e(4).*q(iel).e(1))); 1385 | case {'132', 'xzy', 'XZY', 'ikj'} 1386 | angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)- ... 1387 | q(iel).e(4).*q(iel).e(3)),(q(iel).e(1).^2- ... 1388 | q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2)); 1389 | angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(3)+ ... 1390 | q(iel).e(4).*q(iel).e(1))); 1391 | angles(3,iel) = atan2(2.*(q(iel).e(3).*q(iel).e(1)- ... 1392 | q(iel).e(2).*q(iel).e(4)),(q(iel).e(1).^2+ ... 1393 | q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2)); 1394 | case {'212', 'yxy', 'YXY', 'jij'} 1395 | angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(3)+ ... 1396 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)- ... 1397 | q(iel).e(3).*q(iel).e(4))); 1398 | angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2+ ... 1399 | q(iel).e(3).^2-q(iel).e(4).^2); 1400 | angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(3)- ... 1401 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)+ ... 1402 | q(iel).e(3).*q(iel).e(4))); 1403 | case {'213', 'yxz', 'YXZ', 'jik'} 1404 | angles(1,iel) = atan2(2.*(q(iel).e(3).*q(iel).e(1)- ... 1405 | q(iel).e(4).*q(iel).e(2)),(q(iel).e(1).^2- ... 1406 | q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2)); 1407 | angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(1)+ ... 1408 | q(iel).e(3).*q(iel).e(4))); 1409 | angles(3,iel) = atan2(2.*(q(iel).e(4).*q(iel).e(1)- ... 1410 | q(iel).e(2).*q(iel).e(3)),(q(iel).e(1).^2- ... 1411 | q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2)); 1412 | case {'231', 'yzx', 'YZX', 'jki'} 1413 | angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(4)+ ... 1414 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(1).^2+ ... 1415 | q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2)); 1416 | angles(2,iel) = asin(2.*(q(iel).e(4).*q(iel).e(1)- ... 1417 | q(iel).e(2).*q(iel).e(3))); 1418 | angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)+ ... 1419 | q(iel).e(3).*q(iel).e(4)),(q(iel).e(1).^2- ... 1420 | q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2)); 1421 | case {'232', 'yzy', 'YZY', 'jkj'} 1422 | angles(1,iel) = atan2((q(iel).e(3).*q(iel).e(4)- ... 1423 | q(iel).e(2).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(3)+ ... 1424 | q(iel).e(4).*q(iel).e(1))); 1425 | angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2+ ... 1426 | q(iel).e(3).^2-q(iel).e(4).^2); 1427 | angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(1)+ ... 1428 | q(iel).e(3).*q(iel).e(4)),(q(iel).e(4).*q(iel).e(1)- ... 1429 | q(iel).e(2).*q(iel).e(3))); 1430 | case {'312', 'zxy', 'ZXY', 'kij'} 1431 | angles(1,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(3)+ ... 1432 | q(iel).e(4).*q(iel).e(1)),(q(iel).e(1).^2- ... 1433 | q(iel).e(2).^2+q(iel).e(3).^2-q(iel).e(4).^2)); 1434 | angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(1)- ... 1435 | q(iel).e(3).*q(iel).e(4))); 1436 | angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(4)+ ... 1437 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(1).^2- ... 1438 | q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2)); 1439 | case {'313', 'zxz', 'ZXZ', 'kik'} 1440 | angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(4)- ... 1441 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)+ ... 1442 | q(iel).e(3).*q(iel).e(4))); 1443 | angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2- ... 1444 | q(iel).e(3).^2+q(iel).e(4).^2); 1445 | angles(3,iel) = atan2((q(iel).e(2).*q(iel).e(4)+ ... 1446 | q(iel).e(3).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(1)- ... 1447 | q(iel).e(3).*q(iel).e(4))); 1448 | case {'321', 'zyx', 'ZYX', 'kji'} 1449 | angles(1,iel) = atan2(2.*(q(iel).e(4).*q(iel).e(1)- ... 1450 | q(iel).e(2).*q(iel).e(3)),(q(iel).e(1).^2+ ... 1451 | q(iel).e(2).^2-q(iel).e(3).^2-q(iel).e(4).^2)); 1452 | angles(2,iel) = asin(2.*(q(iel).e(2).*q(iel).e(4)+ ... 1453 | q(iel).e(3).*q(iel).e(1))); 1454 | angles(3,iel) = atan2(2.*(q(iel).e(2).*q(iel).e(1)- ... 1455 | q(iel).e(3).*q(iel).e(4)),(q(iel).e(1).^2- ... 1456 | q(iel).e(2).^2-q(iel).e(3).^2+q(iel).e(4).^2)); 1457 | case {'323', 'zyz', 'ZYZ', 'kjk'} 1458 | angles(1,iel) = atan2((q(iel).e(2).*q(iel).e(1)+ ... 1459 | q(iel).e(3).*q(iel).e(4)),(q(iel).e(3).*q(iel).e(1)- ... 1460 | q(iel).e(2).*q(iel).e(4))); 1461 | angles(2,iel) = acos(q(iel).e(1).^2-q(iel).e(2).^2- ... 1462 | q(iel).e(3).^2+q(iel).e(4).^2); 1463 | angles(3,iel) = atan2((q(iel).e(3).*q(iel).e(4)- ... 1464 | q(iel).e(2).*q(iel).e(1)),(q(iel).e(2).*q(iel).e(4)+ ... 1465 | q(iel).e(3).*q(iel).e(1))); 1466 | otherwise 1467 | error( 'quaternion:EulerAngles:badaxes', ... 1468 | 'Invalid output Euler angle axes' ); 1469 | end % switch axes 1470 | end % for iel 1471 | angles = chop( angles ); 1472 | end % EulerAngles 1473 | 1474 | function q1 = Integral( q0, t, w, varargin ) 1475 | % Integrate angular velocities over time (using ode45) to obtain the 1476 | % orientation quaternions at those times, starting from initial scalar q0. 1477 | % Angular velocities (and rotation axes) are computed at intermediate times 1478 | % by spline interpolation. Use q0 = quaternion(1,0,0,0) as the initial 1479 | % value unless there is an initial orientation. 1480 | % 1481 | % Calling syntax 1: 1482 | % function q1 = Integral( q0, t, w, odeoptions ) 1483 | % Inputs: 1484 | % q0 initial orientation quaternion (normalized, scalar) 1485 | % t(nt) initial and subsequent (or previous) times t = [t0,t1,...] 1486 | % (monotonic) 1487 | % w(3,nt) 3D angular velocity vectors, radians/(unit time) 1488 | % odeoptions [OPTIONAL] ode45 options 1489 | % 1490 | % Calling syntax 2: 1491 | % function q1 = Integral( q0, t, omega, axis, odeoptions ) 1492 | % Inputs: 1493 | % q0 initial orientation quaternion (normalized, scalar) 1494 | % t(nt) initial and subsequent (or previous) times t = [t0,t1,...] 1495 | % (monotonic) 1496 | % omega(nt) angular velocities, radians/(unit time) 1497 | % axis(3,nt) 3D rotation axis vectors (normalized to unit vectors 1498 | % internally) 1499 | % odeoptions [OPTIONAL] ode45 options 1500 | % 1501 | % Output (either syntax): 1502 | % q1(nt) array of normalized quaternions at times t 1503 | % 1504 | % Calls: 1505 | % Derivative quaternion derivative method 1506 | % odeset matlab ode options setter 1507 | % ode45 matlab ode numerical differential equation integrator 1508 | if (nargin > 3) && isnumeric( varargin{1} ) && ... 1509 | (numel( varargin{1} ) >= 3) 1510 | omega = w; 1511 | [axis, dim] = finddim( varargin{1}, 3 ); 1512 | if dim == 0 1513 | error( 'quaternion:Integral:badaxis', ... 1514 | 'axis must have a dimension of size 3' ); 1515 | end 1516 | axis = unitvector( axis(1:3,:), 1 ); 1517 | w = bsxfun( @times, omega(:).', axis ); 1518 | varargin = varargin(2:end); 1519 | end 1520 | [w, dim] = finddim( w, 3 ); 1521 | if dim == 0 1522 | error( 'quaternion:Integral:badw', ... 1523 | 'w must have a dimension of size 3' ); 1524 | end 1525 | w = reshape( w, 3, [] ); 1526 | nw = size( w, 2 ); 1527 | wend = w(:,end); 1528 | nt = numel( t ); 1529 | w = [ w, wend(:,ones(1,nt-nw)) ]; 1530 | if isempty( q0 ) 1531 | y0 = [ 1; 0; 0; 0 ]; 1532 | else 1533 | q0 = q0.normalize; 1534 | y0 = q0.e; 1535 | end 1536 | if isempty( t ) 1537 | nt = nw; 1538 | t = 1 : nt; 1539 | end 1540 | wpp = spline( t, w ); 1541 | options = odeset( varargin{:} ); 1542 | [~, Y] = ode45( @Funct, t, y0, options ); 1543 | function yd = Funct( ti, yi ) 1544 | qi = quaternion( yi(1), yi(2), yi(3), yi(4) ); 1545 | qi = qi.normalize; 1546 | wi = ppval( wpp, ti ); 1547 | yd = double( qi.Derivative( wi )); 1548 | end 1549 | q1 = quaternion( Y(:,1), Y(:,2), Y(:,3), Y(:,4) ); 1550 | q1 = q1.normalize; 1551 | neg = real( q1 ) < 0; 1552 | q1(neg) = -q1(neg); % keep real element >= 0 1553 | q1 = reshape( q1, size(t) ); 1554 | end % Integral 1555 | 1556 | function mrp = ModifiedRodrigues( q ) 1557 | % function mrp = ModifiedRodrigues( q ) or rp = q.ModifiedRodrigues 1558 | % Construct Modified Rodrigues parameters from rotation quaternions 1559 | % Input: 1560 | % q quaternion array 1561 | % Output: 1562 | % mrp Modified Rodrigues parameter array, mrp = axis .* tan(angle/4) 1563 | [angle, axis] = q.AngleAxis; 1564 | if size( angle, 1 ) ~= 1 1565 | angle = shiftdim( angle, -1 ); 1566 | end 1567 | mrp = bsxfun( @times, tan( 0.25 * angle ), axis ); 1568 | end % ModifiedRodrigues 1569 | 1570 | function [omega, axis] = OmegaAxis( q, t, dim ) 1571 | % function [omega, axis] = OmegaAxis( q, t, dim ) or 1572 | % [omega, axis] = q.OmegaAxis( t, dim ) 1573 | % Estimate instantaneous angular velocities and rotation axes from a time 1574 | % series of quaternions. The angular velocity vector omegav is computed by: 1575 | % omegav(:,1) = vector( 2*log( q(1) * inverse(q(2)) )/(t(2) - t(1)) ); 1576 | % omegav(:,i) = vector(... 1577 | % (log( q(i-1) * inverse(q(i)) ) + log( q(i) * inverse(q(i+1))) )/... 1578 | % (0.5*(t(i+1) - t(i-1))) ); 1579 | % omegav(:,end) = vector( 2*log( q(end-1) * inverse(q(end)) )/... 1580 | % (t(end) - t(end-1)) ); 1581 | % [axis, omega] = unitvector( omegav ); 1582 | % Inputs: 1583 | % q array of normalized (rotation) quaternions 1584 | % t [OPT] array of monotonically increasing (or decreasing) times. 1585 | % if omitted or empty, unit time steps are assumed. 1586 | % t must either be a vector with the same length as dimension 1587 | % dim of q, or the same size as q. 1588 | % dim [OPT] dimension of q that is varying in time; if omitted or empty, 1589 | % the first non-singleton dimension is used. 1590 | % Outputs: 1591 | % omega array of instantaneous angular velocities, radians/(unit time) 1592 | % omega >= 0 1593 | % axis instantaneous 3D rotation axis unit vectors at each time 1594 | if isempty( q ) 1595 | omega = []; 1596 | axis = []; 1597 | return; 1598 | end 1599 | if (nargin < 3) || isempty( dim ) 1600 | if (nargin > 1) && ~isempty( t ) 1601 | siq = size( q ); 1602 | sit = size( t ); 1603 | if isequal( siq, sit ) 1604 | dim = find( siq > 1, 1 ); 1605 | else 1606 | dim = find( siq == length( t ), 1 ); 1607 | end 1608 | if isempty( dim ) 1609 | error( 'quaternion:OmegaAxis:notconformable', ... 1610 | 'size of t must agree with at least one dimension of q' ); 1611 | elseif dim > 1 1612 | ndm = ndims( q ); 1613 | perm = [ dim : ndm, 1 : dim-1 ]; 1614 | q = permute( q, perm ); 1615 | if isequal( siq, sit ) 1616 | t = permute( t, perm ); 1617 | end 1618 | end 1619 | else 1620 | [q, dim, perm] = finddim( q, -2 ); 1621 | if dim == 0 1622 | omega = 0; 1623 | axis = unitvector( q.e(2:4), 1 ); 1624 | return; 1625 | end 1626 | end 1627 | elseif dim > 1 1628 | ndm = ndims( q ); 1629 | perm = [ dim : ndm, 1 : dim-1 ]; 1630 | q = permute( q, perm ); 1631 | end 1632 | n = norm( q ); 1633 | if ~all( abs( n(:) - 1 ) < eps(16) ) 1634 | error( 'quaternion:OmegaAxis:qnotnorm', ... 1635 | 'q must be normalized' ); 1636 | end 1637 | siq = size( q ); 1638 | if (nargin < 2) || isempty( t ) 1639 | t = repmat( (0 : (siq(1)-1)).', [1 siq(2:end)] ); 1640 | elseif length( t ) == siq(1) 1641 | t = repmat( t(:), [1 siq(2:end)] ); 1642 | elseif ~isequal( siq, size( t )) 1643 | error( 'quaternion:OmegaAxis:notconformable', ... 1644 | 'size of t must match size of q' ); 1645 | end 1646 | dt = zeros( siq ); 1647 | difft = diff( t, 1 ); 1648 | dt(1,:) = difft(1,:); 1649 | dt(2:end-1,:) = 0.5 *( difft(1:end-1,:) + difft(2:end,:) ); 1650 | dt(end,:) = difft(end,:); 1651 | dq = quaternion.zeros( siq ); 1652 | q1iq2 = q(1:end-1,:) .* inverse( q(2:end,:) ); 1653 | neg = real( q1iq2 ) < 0; 1654 | q1iq2(neg) = -q1iq2(neg); % keep real element >= 0 1655 | derivq = log( q1iq2 ); 1656 | dq(1,:) = 2 .* derivq(1,:); 1657 | dq(2:end-1,:) = derivq(1:end-1,:) + derivq(2:end,:); 1658 | dq(end,:) = 2 .* derivq(end,:); 1659 | omegav = vector( dq ); % angular velocity vectors 1660 | [axis, omega] = unitvector( omegav, 1 ); 1661 | omega = reshape( omega(1,:), siq )./ dt; 1662 | axis = -axis; 1663 | if dim > 1 1664 | axis = ipermute( axis, [1, 1+perm] ); 1665 | omega = ipermute( omega, perm ); 1666 | end 1667 | end % OmegaAxis 1668 | 1669 | function PlotRotation( q, interval ) 1670 | % function PlotRotation( q, interval ) or q.PlotRotation( interval ) 1671 | % Inputs: 1672 | % q quaternion array 1673 | % interval pause between figure updates in seconds, default = 0.1 1674 | % Output: 1675 | % figure plotting the 3 Cartesian axes orientations for the series of 1676 | % quaternions in array q 1677 | if (nargin < 2) || isempty( interval ) 1678 | interval = 0.1; 1679 | end 1680 | nel = numel( q ); 1681 | or = zeros(1,3); 1682 | ax = eye(3); 1683 | alx = zeros( nel, 3, 3 ); 1684 | newplot; 1685 | for iel = 1 : nel 1686 | % plot3( [ or; ax(:,1).' ], [ or ; ax(:,2).' ], [ or; ax(:,3).' ], ':' ); 1687 | plot3( [ or; ax(1,:) ], [ or ; ax(2,:) ], [ or; ax(3,:) ], ':' ); 1688 | hold on 1689 | set( gca, 'Xlim', [-1 1], 'Ylim', [-1 1], 'Zlim', [-1 1] ); 1690 | xlabel( 'x' ); 1691 | ylabel( 'y' ); 1692 | zlabel( 'z' ); 1693 | grid on 1694 | nax = q(iel).RotationMatrix; 1695 | alx(iel,:,:) = nax; 1696 | % plot3( [ or; nax(:,1).' ], [ or ; nax(:,2).' ], [ or; nax(:,3).' ], '-', 'LineWidth', 2 ); 1697 | plot3( [ or; nax(1,:) ], [ or ; nax(2,:) ], [ or; nax(3,:) ], '-', 'LineWidth', 2 ); 1698 | % plot3( alx(1:iel,:,1), alx(1:iel,:,2), alx(1:iel,:,3), '*' ); 1699 | plot3( squeeze(alx(1:iel,1,:)), squeeze(alx(1:iel,2,:)), squeeze(alx(1:iel,3,:)), '*' ); 1700 | if interval 1701 | pause( interval ); 1702 | end 1703 | hold off 1704 | end 1705 | end % PlotRotation 1706 | 1707 | function [q1, w1, t1] = PropagateEulerEq( q0, w0, I, t, torque, varargin ) 1708 | % function [q1, w1, t1] = PropagateEulerEq( q0, w0, I, t, torque, odeoptions ) 1709 | % Inputs: 1710 | % q0 initial orientation quaternion (normalized, scalar) 1711 | % w0(3) initial body frame angular velocity vector 1712 | % I(3) principal body moments of inertia (if no torque, only 1713 | % ratios of elements of I are used) 1714 | % t(nt) initial and subsequent (or previous) times t = [t0,t1,...] 1715 | % (monotonic) 1716 | % @torque [OPTIONAL] function handle to calculate torque vector: 1717 | % tau(1:3) = torque( t, y ), where y = [q.e(1:4); w(1:3)] 1718 | % odeoptions [OPTIONAL] ode45 options 1719 | % Outputs: 1720 | % q1(1,nt) array of normalized quaternions at times t1 1721 | % w1(3,nt) array of body frame angular velocity vectors at times t1 1722 | % t1(1,nt) array of output times 1723 | % Calls: 1724 | % Derivative quaternion derivative method 1725 | % odeset matlab ode options setter 1726 | % ode45 matlab ode numerical differential equation integrator 1727 | % torque [OPTIONAL] user-supplied torque as function of time, orientation, 1728 | % and angular rates; default is no torque 1729 | % Author: 1730 | % Mark Tincknell, 20 December 2010 1731 | % modified 25 July 2012, enforce normalization of q0 and q1 1732 | options = odeset( varargin{:} ); 1733 | q0 = q0.normalize; 1734 | y0 = [q0.e; w0(:)]; 1735 | I0 = [ (I(2) - I(3)) / I(1); 1736 | (I(3) - I(1)) / I(2); 1737 | (I(1) - I(2)) / I(3) ]; 1738 | [T, Y] = ode45( @Euler, t, y0, options ); 1739 | function yd = Euler( ti, yi ) 1740 | qi = quaternion( yi(1), yi(2), yi(3), yi(4) ); 1741 | wi = yi(5:7); 1742 | qd = double( qi.Derivative( wi )); 1743 | wd = [ wi(2) * wi(3) * I0(1); 1744 | wi(3) * wi(1) * I0(2); 1745 | wi(1) * wi(2) * I0(3) ]; 1746 | if exist( 'torque', 'var' ) && isa( torque, 'function_handle' ) 1747 | tau = torque( ti, yi ); 1748 | wd = tau(:) ./ I + wd; 1749 | end 1750 | yd = [ qd; wd ]; 1751 | end 1752 | if numel(t) == 2 1753 | nT = 2; 1754 | T = [T(1); T(end)]; 1755 | Y = [Y(1,:); Y(end,:)]; 1756 | else 1757 | nT = length(T); 1758 | end 1759 | q1 = repmat( quaternion, [1 nT] ); 1760 | w1 = zeros( [3 nT] ); 1761 | t1 = T(:).'; 1762 | for it = 1 : nT 1763 | q1(it) = quaternion( Y(it,1), Y(it,2), Y(it,3), Y(it,4) ); 1764 | w1(:,it) = Y(it,5:7).'; 1765 | end 1766 | q1 = q1.normalize; 1767 | neg = real( q1 ) < 0; 1768 | q1(neg) = -q1(neg); % keep real element >= 0 1769 | end % PropagateEulerEq 1770 | 1771 | function rp = Rodrigues( q ) 1772 | % function rp = Rodrigues( q ) or rp = q.Rodrigues 1773 | % Construct Rodrigues parameters from rotation quaternions 1774 | % Input: 1775 | % q quaternion array 1776 | % Output: 1777 | % rp Rodrigues parameter array, rp = axis .* tan(angle/2) 1778 | [angle, axis] = q.AngleAxis; 1779 | if size( angle, 1 ) ~= 1 1780 | angle = shiftdim( angle, -1 ); 1781 | end 1782 | rp = bsxfun( @times, tan( 0.5 * angle ), axis ); 1783 | end % Rodrigues 1784 | 1785 | function Tp = RotateTensor( varargin ) 1786 | % function Tp = RotateTensor( q, T ) or Tp = q.RotateTensor( T ) 1787 | % 3x3 rotation matrices R are created from q and matrix multiplication 1788 | % rotates 3x3 matrices T into Tp: Tp = R * T * R.'. 1789 | % Inputs: 1790 | % q quaternion array 1791 | % T 3x3xN element Cartesian tensors 1792 | % Output: 1793 | % Tp 3x3xN element rotated tensors 1794 | if nargin < 2 1795 | error( 'quaternion:RotateTensor:noargs', ... 1796 | 'RotateTensor requires 2 inputs: a 3x3xN array and a quaternion' ); 1797 | end 1798 | if isa( varargin{1}, 'quaternion' ) 1799 | q = varargin{1}; 1800 | T = varargin{2}; 1801 | else 1802 | T = varargin{1}; 1803 | q = varargin{2}; 1804 | end 1805 | siT = [size( T ), 1]; 1806 | if ~all( siT(1:2) == [3 3] ) 1807 | error( 'quaternion:RotateTensor:not3x3', ... 1808 | 'Dimension of T must be 3x3xN' ); 1809 | end 1810 | neT = prod( siT(3:end) ); 1811 | neq = numel( q ); 1812 | R = q.RotationMatrix; 1813 | if (neq == neT) || (neq == 1) 1814 | Tp = zeros( siT ); 1815 | for iel = 1 : neT 1816 | Rm = R(:,:,min(neq,iel)); 1817 | Tp(:,:,iel) = Rm * T(:,:,iel) * Rm.'; 1818 | end 1819 | elseif neT == 1 1820 | Tp = zeros( [3, 3, neq] ); 1821 | for iel = 1 : neq 1822 | Tp(:,:,iel) = R(:,:,iel) * T * R(:,:,iel).'; 1823 | end 1824 | else 1825 | error( 'quaternion:RotateTensor:notconformable', ... 1826 | 'q and T must have compatible dimensions' ); 1827 | end 1828 | end % RotateTensor 1829 | 1830 | function vp = RotateVector( varargin ) 1831 | % function vp = RotateVector( q, v, dim ) or 1832 | % vp = q.RotateVector( v, dim ) 1833 | % 3x3 rotation matrices are created from q and matrix multiplication 1834 | % rotates v into vp. RotateVector is 7 times faster than RotateVectorQ. 1835 | % Inputs: 1836 | % q quaternion array 1837 | % v 3xN or Nx3 element Cartesian vectors 1838 | % dim [OPTIONAL] dimension of v with size 3 to rotate 1839 | % Output: 1840 | % vp 3xN or Nx3 element rotated vectors 1841 | if nargin < 2 1842 | error( 'quaternion:RotateVector:noargs', ... 1843 | 'RotateVector requires 2 inputs: a vector and a quaternion' ); 1844 | end 1845 | if isa( varargin{1}, 'quaternion' ) 1846 | q = varargin{1}; 1847 | v = varargin{2}; 1848 | else 1849 | v = varargin{1}; 1850 | q = varargin{2}; 1851 | end 1852 | if (nargin > 2) && ~isempty( varargin{3} ) 1853 | dim = varargin{3}; 1854 | if size( v, dim ) ~= 3 1855 | error( 'quaternion:RotateVector:not3D', ... 1856 | 'Dimension dim of vector v must be size 3' ); 1857 | end 1858 | if dim > 1 1859 | ndm = ndims( v ); 1860 | perm = [ dim : ndm, 1 : dim-1 ]; 1861 | v = permute( v, perm ); 1862 | end 1863 | else 1864 | [v, dim, perm] = finddim( v, 3 ); 1865 | if dim == 0 1866 | error( 'quaternion:RotateVector:not3D', ... 1867 | 'v must have a dimension of size 3' ); 1868 | end 1869 | end 1870 | sip = size( v ); 1871 | v = reshape( v, 3, [] ); 1872 | nev = prod( sip )/ 3; 1873 | R = q.RotationMatrix; 1874 | siq = size( q ); 1875 | neq = prod( siq ); 1876 | if neq == nev 1877 | vp = zeros( sip ); 1878 | for iel = 1 : neq 1879 | vp(:,iel) = R(:,:,iel) * v(:,iel); 1880 | end 1881 | if dim > 1 1882 | vp = ipermute( vp, perm ); 1883 | end 1884 | elseif nev == 1 1885 | siz = [3 siq]; 1886 | vp = zeros( siz ); 1887 | for iel = 1 : neq 1888 | vp(:,iel) = R(:,:,iel) * v; 1889 | end 1890 | if siz(2) == 1 1891 | vp = squeeze( vp ); 1892 | end 1893 | elseif neq == 1 1894 | vp = R * v; 1895 | vp = reshape( vp, sip ); 1896 | if dim > 1 1897 | vp = ipermute( vp, perm ); 1898 | end 1899 | else 1900 | error( 'quaternion:RotateVector:notconformable', ... 1901 | 'q and v must have compatible dimensions' ); 1902 | end 1903 | end % RotateVector 1904 | 1905 | function vp = RotateVectorQ( varargin ) 1906 | % function vp = RotateVectorQ( q, v, dim ) or 1907 | % vp = q.RotateVectorQ( v, dim ) 1908 | % quaternions are created from v and quaternion multiplication rotates v 1909 | % into vp. RotateVector is 7 times faster than RotateVectorQ. 1910 | % Inputs: 1911 | % q quaternion array 1912 | % v 3xN or Nx3 element Cartesian vectors 1913 | % dim [OPTIONAL] dimension of v with size 3 to rotate 1914 | % Output: 1915 | % vp 3xN or Nx3 element rotated vectors 1916 | if nargin < 2 1917 | error( 'quaternion:RotateVectorQ:noargs', ... 1918 | 'RotateVectorQ requires 2 inputs: a vector and a quaternion' ); 1919 | end 1920 | if isa( varargin{1}, 'quaternion' ) 1921 | q = varargin{1}; 1922 | v = varargin{2}; 1923 | else 1924 | v = varargin{1}; 1925 | q = varargin{2}; 1926 | end 1927 | siv = size( v ); 1928 | if (nargin > 2) && ~isempty( varargin{3} ) 1929 | dim = varargin{3}; 1930 | if size( v, dim ) ~= 3 1931 | error( 'quaternion:RotateVectorQ:not3D', ... 1932 | 'Dimension dim of vector v must be size 3' ); 1933 | end 1934 | if dim > 1 1935 | ndm = ndims( v ); 1936 | perm = [ dim : ndm, 1 : dim-1 ]; 1937 | v = permute( v, perm ); 1938 | end 1939 | else 1940 | [v, dim, perm] = finddim( v, 3 ); 1941 | if dim == 0 1942 | error( 'quaternion:RotateVectorQ:not3D', ... 1943 | 'v must have a dimension of size 3' ); 1944 | end 1945 | end 1946 | sip = size( v ); 1947 | qv = quaternion( v(1,:), v(2,:), v(3,:) ); 1948 | qv = reshape( qv, [1 sip(2:end)] ); 1949 | if dim > 1 1950 | qv = ipermute( qv, perm ); 1951 | end 1952 | q = q.normalize; 1953 | qp = q .* qv .* q.conj; 1954 | dp = qp.double; 1955 | nev = prod( siv )/ 3; 1956 | sqz = false; 1957 | if nev == 1 1958 | siz = [3 size(q)]; 1959 | if siz(2) == 1 1960 | sqz = true; 1961 | end 1962 | else 1963 | siz = siv; 1964 | end 1965 | vp = reshape( dp(2:4,:), siz ); 1966 | if sqz 1967 | vp = squeeze( vp ); 1968 | end 1969 | end % RotateVectorQ 1970 | 1971 | function R = RotationMatrix( q ) 1972 | % function R = RotationMatrix( q ) or R = q.RotationMatrix 1973 | % Construct rotation (or direction cosine) matrices from quaternions 1974 | % Input: 1975 | % q quaternion array 1976 | % Output: 1977 | % R 3x3xN rotation (or direction cosine) matrices 1978 | siz = size( q ); 1979 | R = zeros( [3 3 siz] ); 1980 | nel = prod( siz ); 1981 | q = normalize( q ); 1982 | for iel = 1 : nel 1983 | e11 = q(iel).e(1)^2; 1984 | e12 = q(iel).e(1) * q(iel).e(2); 1985 | e13 = q(iel).e(1) * q(iel).e(3); 1986 | e14 = q(iel).e(1) * q(iel).e(4); 1987 | e22 = q(iel).e(2)^2; 1988 | e23 = q(iel).e(2) * q(iel).e(3); 1989 | e24 = q(iel).e(2) * q(iel).e(4); 1990 | e33 = q(iel).e(3)^2; 1991 | e34 = q(iel).e(3) * q(iel).e(4); 1992 | e44 = q(iel).e(4)^2; 1993 | R(:,:,iel) = ... 1994 | [ e11 + e22 - e33 - e44, 2*(e23 - e14), 2*(e24 + e13); ... 1995 | 2*(e23 + e14), e11 - e22 + e33 - e44, 2*(e34 - e12); ... 1996 | 2*(e24 - e13), 2*(e34 + e12), e11 - e22 - e33 + e44 ]; 1997 | end 1998 | R = chop( R ); 1999 | end % RotationMatrix 2000 | end % methods 2001 | 2002 | % Static methods 2003 | methods(Static) 2004 | function q = angleaxis( angle, axis ) 2005 | % function q = quaternion.angleaxis( angle, axis ) 2006 | % Construct quaternions from rotation axes and rotation angles 2007 | % Inputs: 2008 | % angle array of rotation angles in radians 2009 | % axis 3xN or Nx3 array of axes (need not be unit vectors) 2010 | % Output: 2011 | % q quaternion array 2012 | sig = size( angle ); 2013 | six = size( axis ); 2014 | [axis, dim, perm] = finddim( axis, 3 ); 2015 | if dim == 0 2016 | error( 'quaternion:angleaxis:not3D', ... 2017 | 'axis must have a dimension of size 3' ); 2018 | end 2019 | neg = prod( sig ); 2020 | nex = prod( six )/ 3; 2021 | if neg == 1 2022 | siz = six; 2023 | siz(dim)= 1; 2024 | nel = nex; 2025 | elseif nex == 1 2026 | siz = sig; 2027 | nel = neg; 2028 | elseif nex == neg 2029 | siz = sig; 2030 | nel = neg; 2031 | else 2032 | error( 'quaternion:angleaxis:notconformable', ... 2033 | 'angle and axis must have compatible sizes' ); 2034 | end 2035 | for iel = nel : -1 : 1 2036 | d(:,iel) = AngAxis2e( angle(min(iel,neg)), axis(:,min(iel,nex)) ); 2037 | end 2038 | q = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ); 2039 | q = reshape( q, siz ); 2040 | if neg == 1 2041 | q = ipermute( q, perm ); 2042 | end 2043 | end % quaternion.angleaxis 2044 | 2045 | function q = complexmatrix( Q ) 2046 | % function q = quaternion.complexmatrix( Q ) 2047 | % Construct quaternions from special complex 2x2 matrices. Matrix algebra 2048 | % with Q (e.g. sum, difference, matrix product, matrix inverse) is 2049 | % equivalent to the same operation with quaternions. Complex matrices can 2050 | % be created from quaternions with q.ComplexMatrix (upper case). 2051 | % Input: 2052 | % Q 2x2xN array of complex matrices, [a, c; b, d] 2053 | % Output: 2054 | % q array of N quaternions 2055 | % q.e(1) = (d + a)/2, q.e(2) = i*(d - a)/2, 2056 | % q.e(3) = (c - b)/2, q.e(4) = i*(c + b)/2 2057 | siQ = [size( Q ), 1, 1]; 2058 | q1 = 0.5 *( Q(2,2,:) + Q(1,1,:) ); 2059 | q2 = 0.5 *( Q(2,2,:) - Q(1,1,:) )* 1i; 2060 | q3 = 0.5 *( Q(1,2,:) - Q(2,1,:) ); 2061 | q4 = 0.5 *( Q(1,2,:) + Q(2,1,:) )* 1i; 2062 | q = reshape( quaternion( q1, q2, q3, q4 ), siQ(3:end) ); 2063 | end % complexmatrix 2064 | 2065 | function q = eulerangles( varargin ) 2066 | % function q = quaternion.eulerangles( axes, angles ) or 2067 | % function q = quaternion.eulerangles( axes, ang1, ang2, ang3 ) 2068 | % Construct quaternions from triplets of axes and Euler angles 2069 | % Inputs: 2070 | % axes string array or cell string array 2071 | % '123' = 'xyz' = 'XYZ' = 'ijk', etc. 2072 | % angles 3xN or Nx3 array of angles in radians OR 2073 | % ang1, ang2, ang3 arrays of angles in radians 2074 | % Output: 2075 | % q quaternion array 2076 | ics = cellfun( @ischar, varargin ); 2077 | if any( ics ) 2078 | varargin{ics} = cellstr( varargin{ics} ); 2079 | else 2080 | ics = cellfun( @iscellstr, varargin ); 2081 | end 2082 | siv = cellfun( @size, varargin, 'UniformOutput', false ); 2083 | axes = varargin{ics}; 2084 | six = siv{ics}; 2085 | nex = prod( six ); 2086 | dim = 1; 2087 | 2088 | if nargin == 2 % angles is 3xN or Nx3 array 2089 | angles = varargin{~ics}; 2090 | sig = siv{~ics}; 2091 | [angles, dim, perm] = finddim( angles, 3 ); 2092 | if dim == 0 2093 | error( 'quaternion:eulerangles:badaxes', ... 2094 | 'Must supply 3 Euler angles' ); 2095 | end 2096 | sig(dim) = 1; 2097 | neg = prod( sig ); 2098 | if nex == 1 2099 | siz = sig; 2100 | elseif neg == 1 2101 | siz = six; 2102 | elseif nex == neg 2103 | siz = sig; 2104 | end 2105 | nel = prod( siz ); 2106 | for iel = nel : -1 : 1 2107 | q(iel) = EulerAng2q( axes{min(iel,nex)}, ... 2108 | angles(:,min(iel,neg)) ); 2109 | end 2110 | 2111 | elseif nargin == 4 % each of 3 angles is separate input argument 2112 | angles = varargin(~ics); 2113 | na = cellfun( 'prodofsize', angles ); 2114 | [neg, jeg] = max( na ); 2115 | if ~all( (na == 1) | (na == neg) ) 2116 | error( 'quaternion:eulerangles:notconformable', ... 2117 | 'All angles must be singletons or have the same number of elements' ); 2118 | end 2119 | sig = size( angles{jeg} ); 2120 | if nex == 1 2121 | siz = sig; 2122 | elseif neg == 1 2123 | siz = six; 2124 | elseif nex == neg 2125 | siz = sig; 2126 | end 2127 | nel = prod( siz ); 2128 | for iel = nel : -1 : 1 2129 | q(iel) = EulerAng2q( axes{min(iel,nex)}, ... 2130 | [angles{1}(min(iel,na(1))), ... 2131 | angles{2}(min(iel,na(2))), ... 2132 | angles{3}(min(iel,na(3)))] ); 2133 | end 2134 | else 2135 | error( 'quaternion:eulerangles:badinputs', ... 2136 | 'Must supply either 2 or 4 input arguments' ); 2137 | end % if nargin 2138 | 2139 | q = reshape( q, siz ); 2140 | if (dim > 1) && isequal( siz, sig ) 2141 | q = ipermute( q, perm ); 2142 | end 2143 | if ~ismatrix( q ) && (size( q, 1 ) == 1) 2144 | q = shiftdim( q, 1 ); 2145 | end 2146 | end % quaternion.eulerangles 2147 | 2148 | function q = eye( N ) 2149 | % function q = eye( N ) 2150 | if nargin < 1 2151 | N = 1; 2152 | end 2153 | if isempty(N) || (N <= 0) 2154 | q = quaternion.empty; 2155 | else 2156 | q = quaternion( eye(N), 0, 0, 0 ); 2157 | end 2158 | end % quaternion.eye 2159 | 2160 | function q = integrateomega( varargin ) 2161 | % Integrate angular velocities over time (using ode45) to obtain the 2162 | % orientation quaternions at those times, using quaternion.Integral and 2163 | % initial quaternion(1,0,0,0) 2164 | % 2165 | % Calling syntax 1: 2166 | % function q = quaternion.integrateomega( t, w, odeoptions ) 2167 | % Inputs: 2168 | % t(nt) initial and subsequent (or previous) times t = [t0,t1,...] 2169 | % (monotonic) 2170 | % w(3,nt) 3D angular velocity vectors, radians/(unit time) 2171 | % odeoptions [OPTIONAL] ode45 options 2172 | % 2173 | % Calling syntax 2: 2174 | % function q = quaternion.integrateomega( t, omega, axis, odeoptions ) 2175 | % Inputs: 2176 | % t(nt) initial and subsequent (or previous) times t = [t0,t1,...] 2177 | % (monotonic) 2178 | % omega(nt) angular velocities, radians/(unit time) 2179 | % axis(3,nt) 3D rotation axis vectors (normalized to unit vectors 2180 | % internally) 2181 | % odeoptions [OPTIONAL] ode45 options 2182 | % 2183 | % Output (either syntax): 2184 | % q(nt) array of normalized quaternions at times t 2185 | % 2186 | % Calls: 2187 | % Integral quaternion integral method 2188 | q = Integral( quaternion(1,0,0,0), varargin{:} ); 2189 | end % integrateomega 2190 | 2191 | function q = modifiedrodrigues( mrp ) 2192 | % function q = quaternion.modifiedrodrigues( mrp ) 2193 | % Construct rotation quaternions from Modified Rodrigues parameters 2194 | % Input: 2195 | % mrp array of Modified Rodrigues parameters, mrp = axis.*tan(angle/4) 2196 | % Output: 2197 | % q array of normalized (rotation) quaternions 2198 | [mrp, dim, perm] = finddim( mrp, 3 ); 2199 | if dim == 0 2200 | error( 'quaternion:modifiedrodrigues:badpar', ... 2201 | 'Input must have a dimension of length 3' ); 2202 | end 2203 | [axis, magn] = unitvector( mrp ); 2204 | siz = size( magn ); 2205 | angle = 4 * atan( reshape( magn(1,:), [1, siz(2:end)] )); 2206 | q = quaternion.angleaxis( angle, axis ); 2207 | if dim > 1 2208 | q = ipermute( q, perm ); 2209 | end 2210 | end % modifiedrodrigues 2211 | 2212 | function q = nan( varargin ) 2213 | % function q = quaternion.nan( siz ) 2214 | if isempty( varargin ) 2215 | siz = [1 1]; 2216 | elseif numel( varargin ) > 1 2217 | siz = [varargin{:}]; 2218 | elseif isempty( varargin{1} ) 2219 | siz = [0 0]; 2220 | elseif numel( varargin{1} ) > 1 2221 | siz = varargin{1}; 2222 | else 2223 | siz = [varargin{1} varargin{1}]; 2224 | end 2225 | if prod( siz ) == 0 2226 | q = reshape( quaternion.empty, siz ); 2227 | else 2228 | q = quaternion( nan(siz), nan, nan, nan ); 2229 | end 2230 | end % quaternion.nan 2231 | 2232 | function q = NaN( varargin ) 2233 | % function q = quaternion.NaN( siz ) 2234 | q = quaternion.nan( varargin{:} ); 2235 | end % quaternion.NaN 2236 | 2237 | function q = ones( varargin ) 2238 | % function q = quaternion.ones( siz ) 2239 | if isempty( varargin ) 2240 | siz = [1 1]; 2241 | elseif numel( varargin ) > 1 2242 | siz = [varargin{:}]; 2243 | elseif isempty( varargin{1} ) 2244 | siz = [0 0]; 2245 | elseif numel( varargin{1} ) > 1 2246 | siz = varargin{1}; 2247 | else 2248 | siz = [varargin{1} varargin{1}]; 2249 | end 2250 | if prod( siz ) == 0 2251 | q = reshape( quaternion.empty, siz ); 2252 | else 2253 | q = quaternion( ones(siz), 0, 0, 0 ); 2254 | end 2255 | end % quaternion.ones 2256 | 2257 | function q = rand( varargin ) 2258 | % function q = quaternion.rand( siz ) 2259 | % Input: 2260 | % siz size of output array q 2261 | % Output: 2262 | % q uniform random quaternions, NOT normalized to 1, 2263 | % 0 <= q.e(1) <= 1, -1 <= q.e(2:4) <= 1 2264 | if isempty( varargin ) 2265 | siz = [1 1]; 2266 | elseif numel( varargin ) > 1 2267 | siz = [varargin{:}]; 2268 | elseif isempty( varargin{1} ) 2269 | siz = [0 0]; 2270 | elseif numel( varargin{1} ) > 1 2271 | siz = varargin{1}; 2272 | else 2273 | siz = [varargin{1} varargin{1}]; 2274 | end 2275 | if prod( siz ) == 0 2276 | q = quaternion.empty; 2277 | return; 2278 | end 2279 | d = [ rand( [1, siz] ); 2 * rand( [3, siz] ) - 1 ]; 2280 | q = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ); 2281 | q = reshape( q, siz ); 2282 | end % quaternion.rand 2283 | 2284 | function q = randRot( varargin ) 2285 | % function q = quaternion.randRot( siz ) 2286 | % Random quaternions uniform in rotation space 2287 | % Input: 2288 | % siz size of output array q 2289 | % Output: 2290 | % q random quaternions, normalized to 1, 0 <= q.e(1) <= 1, 2291 | % uniform over the 3D surface of a 4 dimensional hypersphere 2292 | if isempty( varargin ) 2293 | siz = [1 1]; 2294 | elseif numel( varargin ) > 1 2295 | siz = [varargin{:}]; 2296 | elseif isempty( varargin{1} ) 2297 | siz = [0 0]; 2298 | elseif numel( varargin{1} ) > 1 2299 | siz = varargin{1}; 2300 | else 2301 | siz = [varargin{1} varargin{1}]; 2302 | end 2303 | if prod( siz ) == 0 2304 | q = quaternion.empty; 2305 | return; 2306 | end 2307 | d = randn( [4, prod( siz )] ); 2308 | n = sqrt( sum( d.^2, 1 )); 2309 | dn = bsxfun( @rdivide, d, n ); 2310 | neg = dn(1,:) < 0; 2311 | dn(:,neg) = -dn(:,neg); 2312 | q = quaternion( dn(1,:), dn(2,:), dn(3,:), dn(4,:) ); 2313 | q = reshape( q, siz ); 2314 | end % quaternion.randRot 2315 | 2316 | function q = rodrigues( rp ) 2317 | % function q = quaternion.rodrigues( rp ) 2318 | % Construct rotation quaternions from Rodrigues parameters 2319 | % Input: 2320 | % rp array of Rodrigues parameters, rp = axis .* tan(angle/2) 2321 | % Output: 2322 | % q array of normalized (rotation) quaternions 2323 | [rp, dim, perm] = finddim( rp, 3 ); 2324 | if dim == 0 2325 | error( 'quaternion:rodrigues:badpar', ... 2326 | 'Input must have a dimension of length 3' ); 2327 | end 2328 | [axis, magn] = unitvector( rp ); 2329 | siz = size( magn ); 2330 | angle = 2 * atan( reshape( magn(1,:), [1, siz(2:end)] )); 2331 | q = quaternion.angleaxis( angle, axis ); 2332 | if dim > 1 2333 | q = ipermute( q, perm ); 2334 | end 2335 | end % rodrigues 2336 | 2337 | function q = rotateutov( u, v, dimu, dimv ) 2338 | % function q = quaternion.rotateutov( u, v, dimu, dimv ) 2339 | % Construct quaternions to rotate vectors u into directions of vectors v 2340 | % Inputs: 2341 | % u 3x1 or 3xN or 1x3 or Nx3 arrays of vectors 2342 | % v 3x1 or 3xN or 1x3 or Nx3 arrays of vectors 2343 | % dimu [OPTIONAL] dimension of u with size 3 to use 2344 | % dimv [OPTIONAL] dimension of v with size 3 to use 2345 | % Output: 2346 | % q quaternion array 2347 | if (nargin < 3) || isempty( dimu ) 2348 | [u, dimu, permu] = finddim( u, 3 ); 2349 | if dimu == 0 2350 | error( 'quaternion:rotateutov:not3D', ... 2351 | 'u must have a dimension of size 3' ); 2352 | end 2353 | elseif dimu > 1 2354 | ndmu = ndims( u ); 2355 | permu = [ dimu : ndmu, 1 : dimu-1 ]; 2356 | u = permute( u, permu ); 2357 | else 2358 | permu = 1 : ndims(u); 2359 | end 2360 | siu = size( u ); 2361 | siu(1) = 1; 2362 | neu = prod( siu ); 2363 | if (nargin < 4) || isempty( dimv ) 2364 | [v, dimv, permv] = finddim( v, 3 ); 2365 | if dimv == 0 2366 | error( 'quaternion:rotateutov:not3D', ... 2367 | 'v must have a dimension of size 3' ); 2368 | end 2369 | elseif dimv > 1 2370 | ndmv = ndims( v ); 2371 | permv = [ dimv : ndmv, 1 : dimv-1 ]; 2372 | v = permute( v, permv ); 2373 | else 2374 | permv = 1 : ndims(v); 2375 | end 2376 | siv = size( v ); 2377 | siv(1) = 1; 2378 | nev = prod( siv ); 2379 | if neu == nev 2380 | siz = siu; 2381 | nel = neu; 2382 | perm = permu; 2383 | dim = dimu; 2384 | elseif (neu > 1) && (nev == 1) 2385 | siz = siu; 2386 | nel = neu; 2387 | perm = permu; 2388 | dim = dimu; 2389 | elseif (neu == 1) && (nev > 1) 2390 | siz = siv; 2391 | nel = nev; 2392 | perm = permv; 2393 | dim = dimv; 2394 | else 2395 | error( 'quaternion:rotateutov:notconformable', ... 2396 | 'Number of 3 element vectors in u and v must be 1 or equal' ); 2397 | end 2398 | for iel = nel : -1 : 1 2399 | q(iel) = UV2q( u(:,min(iel,neu)), v(:,min(iel,nev)) ); 2400 | end 2401 | if dim > 1 2402 | q = ipermute( reshape( q, siz ), perm ); 2403 | end 2404 | end % quaternion.rotateutov 2405 | 2406 | function q = rotationmatrix( R ) 2407 | % function q = quaternion.rotationmatrix( R ) 2408 | % Construct quaternions from rotation (or direction cosine) matrices 2409 | % Input: 2410 | % R 3x3xN rotation (or direction cosine) matrices 2411 | % Output: 2412 | % q quaternion array 2413 | siz = [size(R) 1 1]; 2414 | if ~all( siz(1:2) == [3 3] ) || ... 2415 | (abs( det( R(:,:,1) ) - 1 ) > eps(16) ) 2416 | error( 'quaternion:rotationmatrix:baddet', ... 2417 | 'Rotation matrices must be 3x3xN with det(R) == 1' ); 2418 | end 2419 | nel = prod( siz(3:end) ); 2420 | for iel = nel : -1 : 1 2421 | d(:,iel) = RotMat2e( chop( R(:,:,iel) )); 2422 | end 2423 | q = quaternion( d(1,:), d(2,:), d(3,:), d(4,:) ); 2424 | q = normalize( q ); 2425 | q = reshape( q, siz(3:end) ); 2426 | end % quaternion.rotationmatrix 2427 | 2428 | function q = zeros( varargin ) 2429 | % function q = quaternion.zeros( siz ) 2430 | if isempty( varargin ) 2431 | siz = [1 1]; 2432 | elseif numel( varargin ) > 1 2433 | siz = [varargin{:}]; 2434 | elseif isempty( varargin{1} ) 2435 | siz = [0 0]; 2436 | elseif numel( varargin{1} ) > 1 2437 | siz = varargin{1}; 2438 | else 2439 | siz = [varargin{1} varargin{1}]; 2440 | end 2441 | if prod( siz ) == 0 2442 | q = reshape( quaternion.empty, siz ); 2443 | else 2444 | q = quaternion( zeros(siz), 0, 0, 0 ); 2445 | end 2446 | end % quaternion.zeros 2447 | 2448 | end % methods(Static) 2449 | end % classdef quaternion 2450 | 2451 | % Scalar rotation conversion functions 2452 | function eout = AngAxis2e( angle, axis ) 2453 | % function eout = AngAxis2e( angle, axis ) 2454 | % One Angle-Axis -> one quaternion 2455 | s = sin( 0.5 * angle ); 2456 | v = axis(:); 2457 | vn = norm( v ); 2458 | if vn == 0 2459 | if s == 0 2460 | c = 0; 2461 | else 2462 | c = 1; 2463 | end 2464 | u = zeros( 3, 1 ); 2465 | else 2466 | c = cos( 0.5 * angle ); 2467 | u = v(:) ./ vn; 2468 | end 2469 | eout = [ c; s * u ]; 2470 | if (eout(1) < 0) && (mod( angle/(2*pi), 2 ) ~= 1) 2471 | eout = -eout; % rotationally equivalent quaternion with real element >= 0 2472 | end 2473 | end % AngAxis2e 2474 | 2475 | function qout = EulerAng2q( axes, angles ) 2476 | % function qout = EulerAng2q( axes, angles ) 2477 | % One triplet Euler Angles -> one quaternion 2478 | na = length( axes ); 2479 | axis = zeros( 3, na ); 2480 | for i0 = 1 : na 2481 | switch axes(i0) 2482 | case {'1', 'i', 'x', 'X'} 2483 | axis(:,i0) = [ 1; 0; 0 ]; 2484 | case {'2', 'j', 'y', 'Y'} 2485 | axis(:,i0) = [ 0; 1; 0 ]; 2486 | case {'3', 'k', 'z', 'Z'} 2487 | axis(:,i0) = [ 0; 0; 1 ]; 2488 | otherwise 2489 | error( 'quaternion:EulerAng2q:badaxis', ... 2490 | 'Illegal axis designation' ); 2491 | end 2492 | end 2493 | q0 = quaternion.angleaxis( angles(:).', axis ); 2494 | qout = q0(1); 2495 | for i0 = 2 : numel(q0) 2496 | qout = product( q0(i0), qout ); 2497 | end 2498 | if qout.e(1) < 0 2499 | qout = -qout; % rotationally equivalent quaternion with real element >= 0 2500 | end 2501 | end % EulerAng2q 2502 | 2503 | function eout = RotMat2e( R ) 2504 | % function eout = RotMat2e( R ) 2505 | % One Rotation Matrix -> one quaternion 2506 | eout = zeros( 4, 1 ); 2507 | if ~all( all( R == 0 )) 2508 | eout(1) = 0.5 * sqrt( max( 0, R(1,1) + R(2,2) + R(3,3) + 1 )); 2509 | if eout(1) == 0 2510 | eout(2) = sqrt( max( 0, -0.5 *( R(2,2) + R(3,3) ))) * ... 2511 | sgn( -R(2,3) ); 2512 | eout(3) = sqrt( max( 0, -0.5 *( R(1,1) + R(3,3) ))) * ... 2513 | sgn( -R(1,3) ); 2514 | eout(4) = sqrt( max( 0, -0.5 *( R(1,1) + R(2,2) ))) * ... 2515 | sgn( -R(1,2) ); 2516 | else 2517 | eout(2) = 0.25 *( R(3,2) - R(2,3) )/ eout(1); 2518 | eout(3) = 0.25 *( R(1,3) - R(3,1) )/ eout(1); 2519 | eout(4) = 0.25 *( R(2,1) - R(1,2) )/ eout(1); 2520 | end 2521 | end 2522 | end % RotMat2e 2523 | 2524 | function qout = UV2q( u, v ) 2525 | % function qout = UV2q( u, v ) 2526 | % One pair vectors U, V -> one quaternion 2527 | w = cross( u, v ); % construct vector w perpendicular to u and v 2528 | magw = norm( w ); 2529 | dotuv = dot( u, v ); 2530 | if magw == 0 2531 | % Either norm(u) == 0 or norm(v) == 0 or dotuv/(norm(u)*norm(v)) == 1 2532 | if dotuv >= 0 2533 | qout = quaternion( 1, 0, 0, 0 ); 2534 | return; 2535 | end 2536 | % dotuv/(norm(u)*norm(v)) == -1 2537 | % If v == [v(1); 0; 0], rotate by pi about the [0; 0; 1] axis 2538 | if (v(2) == 0) && (v(3) == 0) 2539 | qout = quaternion( 0, 0, 0, 1 ); 2540 | return; 2541 | end 2542 | % Otherwise constuct "what" such that dot(v,what) == 0, and rotate about it 2543 | % by pi 2544 | what = [ 0; -v(3); v(2) ]./ sqrt( v(2)^2 + v(3)^2 ); 2545 | costh = -1; 2546 | else 2547 | % Use w as rotation axis, angle between u and v as rotation angle 2548 | what = w(:) / magw; 2549 | costh = dotuv /( norm(u) * norm(v) ); 2550 | end 2551 | c = sqrt( 0.5 *( 1 + costh )); % real element >= 0 2552 | s = sqrt( 0.5 *( 1 - costh )); 2553 | eout = [ c; s * what ]; 2554 | qout = quaternion( eout(1), eout(2), eout(3), eout(4) ); 2555 | end % UV2q 2556 | 2557 | % Helper functions 2558 | function out = chop( in, tol ) 2559 | % function out = chop( in, tol ) 2560 | % Replace values that differ from an integer by <= tol by the integer 2561 | % Inputs: 2562 | % in input array 2563 | % tol tolerance, default = eps 2564 | % Output: 2565 | % out input array with integer replacements, if any 2566 | if (nargin < 2) || isempty( tol ) 2567 | tol = eps; 2568 | end 2569 | out = in; 2570 | rin = round( in ); 2571 | lx = abs( rin - in ) <= tol; 2572 | out(lx) = rin(lx); 2573 | end % chop 2574 | 2575 | function [aout, dim, perm] = finddim( ain, len ) 2576 | % function [aout, dim, perm] = finddim( ain, len ) 2577 | % Find first dimension in ain of length len, permute ain to make it first 2578 | % Inputs: 2579 | % ain(s1,s2,...) data array, size = [s1, s2, ...] 2580 | % len length sought, e.g. s2 == len 2581 | % if len < 0, then find first dimension >= |len| 2582 | % Outputs: 2583 | % aout(s2,...,s1) data array, permuted so first dimension is length len 2584 | % dim dimension number of length len, 0 if ain has none 2585 | % perm permutation order (for permute and ipermute) of aout, 2586 | % e.g. [2, ..., 1] 2587 | % Notes: if no dimension has length len, aout = ain, dim = 0, perm = 1:ndm 2588 | % ain = ipermute( aout, perm ) 2589 | siz = size( ain ); 2590 | ndm = length( siz ); 2591 | if len < 0 2592 | dim = find( siz >= -len, 1, 'first' ); 2593 | else 2594 | dim = find( siz == len, 1, 'first' ); 2595 | end 2596 | if isempty( dim ) 2597 | dim = 0; 2598 | end 2599 | if dim < 2 2600 | aout = ain; 2601 | perm = 1 : ndm; 2602 | else 2603 | % Permute so that dim becomes the first dimension 2604 | perm = [ dim : ndm, 1 : dim-1 ]; 2605 | aout = permute( ain, perm ); 2606 | end 2607 | end % finddim 2608 | 2609 | function s = sgn( x ) 2610 | % function s = sgn( x ), if x >= 0, s = 1, else s = -1 2611 | s = ones( size( x )); 2612 | s(x < 0) = -1; 2613 | end % sgn 2614 | 2615 | function [u, n] = unitvector( v, dim ) 2616 | % function [u, n] = unitvector( v, dim ) 2617 | % Inputs: 2618 | % v matrix of vectors 2619 | % dim [OPTIONAL] dimension to normalize, dim >= 1 2620 | % if no dim input, use first dimension of length >= 2 2621 | % Outputs: 2622 | % u matrix of unit vectors (except for vectors of norm 0) 2623 | % n matrix same size as v and u of norms 2624 | ndm = ndims( v ); 2625 | if (nargin < 2) || isempty( dim ) 2626 | [v, dim, perm] = finddim( v, -2 ); 2627 | if dim == 0 2628 | n = sqrt( v.*conj(v) ); 2629 | n0 = (n ~= 0) & (n ~= 1); 2630 | u = v; 2631 | u(n0) = v(n0) ./ n(n0); 2632 | return; 2633 | end 2634 | else 2635 | perm = [ dim : ndm, 1 : dim-1 ]; 2636 | v = permute( v, perm ); 2637 | end 2638 | u = v; 2639 | sv = size( v ); 2640 | n = repmat( sqrt( sum( v.*conj(v), 1 )), [sv(1) ones(1,ndm-1)] ); 2641 | n0 = (n ~= 0) & (n ~= 1); 2642 | u(n0) = v(n0) ./ n(n0); 2643 | u = ipermute( u, perm ); 2644 | if nargout > 1 2645 | n = ipermute( n, perm ); 2646 | end 2647 | end % unitvector --------------------------------------------------------------------------------