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

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