https://github.com/RomaTeng/EKF-SLAM-on-Manifold
Tip revision: 12d7d8d88c84161baed173e38d49dedb4adb2b96 authored by Teng Zhang on 26 February 2017, 15:09:17 UTC
Record my control paper
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