YoshiRi

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Euler angle to quaternion 
% input : roll,pitch,yaw [rad]
% output quaternion: 
% you need robotics toolbox
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function Q = eul2quat_self(rpy)

roll = rpy(1);

YoshiRi

///////////////////////////////////////////
//  Yoshi Ri @ Univ Tokyo
//  8 April 2018
//////////////////////////////////////////

// for servo motor control
#include <Servo.h>
#include <Encoder.h>
// ROS communication
//#include <ArduinoHardware.h>

YoshiRi

YoshiRi

YoshiRi

% figure のデフォルトフォント設定
set(0, 'defaultAxesFontSize', 12);
set(0, 'defaultTextFontSize', 12);
set(0, 'defaultAxesFontName','Times-Roman');
set(0,'defaultTextFontName','Times-Roman');
% ブロックのデフォルトフォントを変更
set_param(0,'DefaultBlockFontSize',11);
set_param(0,'DefaultBlockFontName','Times-Roman');
% ラインのデフォルトフォントサイズを変更
set_param(0,'DefaultLineFontSize',9);

YoshiRi

%%  test for particle filter
% Problem Setting is here
% https://ja.wikipedia.org/wiki/%E3%82%AB%E3%83%AB%E3%83%9E%E3%83%B3%E3%83%95%E3%82%A3%E3%83%AB%E3%82%BF%E3%83%BC#%E5%BF%9C%E7%94%A8%E4%BE%8B

% settings
dt = 0.001; % time sample
s_sys = 10000; % system noise
s_out = 0.05; % measurement noise
s_rout = 0.1;
% system eq

YoshiRi

// for image processing
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/xfeatures2d.hpp"
#include "opencv2/flann.hpp"
#include "opencv2/cudaarithm.hpp"
#include "opencv2/cudafeatures2d.hpp"

YoshiRi

clear all
close all

%% config
%true depth
% sampling time 
ST = 0.033 %33ms
END = 5.0 % 5sec simulation
STEREO_NOISE_S = 1; % 1px sigma for stereo disparity noise
MONO_NOISE_S = 1; % 1?? sigma for monocular distance estimation noise

YoshiRi

from ctypes import *
import math
import random
import numpy as np
import matplotlib.pyplot as plt
import time # time.time() to get time

class Checktime:
    def __init__(self):
        self.checktime = [0]

YoshiRi