買書捐殘盟

2011年8月9日 星期二

Matlab for NXT,經卡爾門濾波前後,陀螺儀數據觀測

經卡爾門濾波前後,陀螺儀數據觀測,發現有很明顯的濾波效果,由記錄的結果可證實。

[程式碼]
% 測試陀螺儀元件- GetGyro()

clear all
clc

%IN Port 設定
touch_port = SENSOR_1;
gyroport = SENSOR_3;

%設定Kalman Filter 參數
P=10;
Q=1e-4;
R=0.05;
prev_filter_value=0;

COM_CloseNXT('all');
hNXT=COM_OpenNXT();
COM_SetDefaultNXT(hNXT);

OpenSwitch(touch_port); %開啟觸碰感應器

OpenGyro(gyroport); %開啟陀螺儀感知器
offset = CalibrateGyro(gyroport,'AUTO'); %計算陀螺儀偏移值(offset)

cnt=1;
while(GetSwitch(touch_port)==false)
 GyroValue= GetGyro(gyroport); %陀螺儀原始數據
 [P,curr_filter_value]=Kalman_filter(P,Q,R,prev_filter_value,GyroValue); %陀螺儀數據經卡爾門濾波後的數據
 disp(sprintf('GyroValue=%2.2f',curr_filter_value));
 pause(0.01);
 clc
 data(1,cnt)=GyroValue; %紀錄原始陀螺儀數據
 data(2,cnt)=curr_filter_value; %紀錄卡爾門濾波後的數據
 prev_filter_value=curr_filter_value;
 cnt=cnt+1;
end

CloseSensor(touch_port); %關閉感知器
CloseSensor(gyroport);
COM_CloseNXT(hNXT);

plot(data(1,:))
hold on
plot(data(2,:),'r-')
hold off


[副程式Kalman_Filter.m]

function [P,curr_filter_value]=Kalman_filter(P,Q,R,prev_filter_value,measurement_value)
 P=P+Q; %第k-1時刻與第k時刻前測之間的協方差P
 Kgain=P/(P+R); %卡爾曼增益
 curr_filter_value=prev_filter_value+Kgain*(measurement_value-prev_filter_value);
 P=(1-Kgain)*P; %第k時刻後測與前測之間的協方差
end



[執行結果]
offset=610.2152

從上圖中,藍色線代表未濾波前的數據、紅色線為濾波後的數據,前後結果非常明顯。

沒有留言:

張貼留言