https://github.com/RomaTeng/EKF-SLAM-on-Manifold
Raw File
Tip revision: 12d7d8d88c84161baed173e38d49dedb4adb2b96 authored by Teng Zhang on 26 February 2017, 15:09:17 UTC
Record my control paper
Tip revision: 12d7d8d
gen_trajectory.m
function [ Vehicle ] = gen_trajectory( t )

     radius = 10;   
    Vehicle.position = radius* [ 5*cos(0.3*t/4); 4*sin(0.2*t/4); 2*sin(0.2*t/4+1) ];
    %Vehicle.position = radius* [ 5*cos(0.03*t); 4*sin(0.02*t); 2*sin(0.02*t+1) ];
    Vehicle.euler = [0.5*t+2; -0.3*t; 0.4*t];
    

    
    
    
    
    % generate rotation matrix
    for i = 1:size(Vehicle.euler, 2)
        Vehicle.orientation((i-1)*3+1:i*3, 1:3) = euler2rotation_matrix(Vehicle.euler(:, i));
    end
end

back to top