00001 clear all; close all;
00002
00003 input = load('input_new.txt');
00004
00005 sample_freq = 100;
00006 desired_freq = 20;
00007 resample = 20;
00008 butter_factor = desired_freq / sample_freq;
00009 resample_factor = resample / sample_freq;
00010
00011 [B A] = butter(7, butter_factor, 'low');
00012 [size_input tmp] = size(input);
00013 counter = 1;
00014
00015 time = input(:,counter);
00016 time_filter = time(1:1/resample_factor:size_input,:);
00017 counter = counter +1;
00018
00019 Wrench_fs_fs = input(:,counter:counter+5);
00020 Wrench_fs_fs_filter = lowpass(B, A, resample_factor, Wrench_fs_fs);
00021 counter = counter +6;
00022
00023 Wrench_world_world = input(:,counter:counter+5);
00024 Wrench_world_world_filter = lowpass(B, A, resample_factor, Wrench_world_world);
00025 counter = counter +6;
00026
00027 Wrench_obj_obj = input(:,counter:counter+5);
00028 Wrench_obj_obj_filter = lowpass(B, A, resample_factor, Wrench_obj_obj);
00029 counter = counter +6;
00030
00031 Twist_world_world = input(:,counter:counter+5);
00032 Twist_world_world_filter = lowpass(B, A, resample_factor, Twist_world_world);
00033 counter = counter +6;
00034
00035 Twist_obj_world = input(:,counter:counter+5);
00036 Twist_obj_world_filter = lowpass(B, A, resample_factor, Twist_obj_world);
00037 counter = counter +6;
00038
00039 Frame_world_obj = input(:,counter:counter+11);
00040 Frame_world_obj_filter = lowpass(B, A, resample_factor, Frame_world_obj);
00041 counter = counter +12;
00042
00043 leds = input(:,counter);
00044 leds_filter = leds(1:1/resample_factor:size_input,:);
00045
00046
00047 filterdata = [time_filter Frame_world_obj_filter Wrench_world_world_filter Twist_world_world_filter leds_filter];
00048
00049 save -ASCII filterdata.txt filterdata;
00050
00051 figure;
00052 plot(time, Frame_world_obj); title('Frame world obj');
00053 figure;
00054 plot(time_filter, Frame_world_obj_filter); title('Frame world obj filter');
00055
00056 figure;
00057 plot(time, Twist_world_world); title('Twist world world');
00058 figure;
00059 plot(time_filter, Twist_world_world_filter); title('Twist world world filter');
00060
00061 figure;
00062 plot(time, Wrench_obj_obj); title('Wrench obj obj');
00063 figure;
00064 plot(time_filter, Wrench_obj_obj_filter); title('Wrench obj obj filter');
00065
00066 figure;
00067 plot(time, Wrench_world_world); title('Wrench world world');
00068 figure;
00069 plot(time_filter, Wrench_world_world_filter); title('Wrench world world filter');
00070
00071 figure;
00072 plot(time, leds); title('Num Leds');
00073
00074
00075 Frame_world_obj
