買書捐殘盟

2011年5月31日 星期二

Runge-Kutta應用於NXT-兩輪平衡車角度的量測

陀螺儀對機身擾動非常敏感,在前述透過卡爾門濾波器的設計,可以有效減低它的敏
感性。
目前進行陀螺儀對二輪平衡車測定其傾斜機身角度,運用離散時間之轉角速度與時間
乘積加總所得的角度,會有誤差的問題之外,陀螺儀在運作期間,也會因為飄移的問題
,使角度誤差的情況更加嚴重(換句話說:當二輪平衡車在前傾與後仰的維持平衡一
段時間後,當機身回到直立狀態的某一瞬間,其角度並非零值,可能為負或正),此
刻易使控制器產生誤判而使機身搖擺。
所以程式中,另行加入Runge-Kutta的二階微分技術,除了使測定的角度更加精準之
外,尚須扣除角度的偏移值(bias),將之應用在NXT,明顯改善本問題。
[NXC版本]
#define GyroScopePort IN_3
#define sample_size 100
#define GyroScale 4
#define t_scale 1e+6

//Define gyro offset parameters
float gyro_sum=0, gyro_avg=0;
int gyro_value=0;
int offset=0;
int i;

//Define Kalman filter parameter
float P=10; //Process state covariance
float Kgain; //The Kalman Filter Gain
float X_old=0;
float X_curr;
float Q=0.0001; //The Probability of process noise w ,P(w)~N(0,Q)
float R=0.1; //The Probability of measurement noise v, P(v)~N(0,R)

//Define BodyTiltTheta parameters
float theta=0,prev_theta=0;
float f1=0,f2=0;
float aa=0.001;
float theta_bias=0;
long prev_tick;


int KalmanFilter(int gyro_value)
{
X_curr=X_old;
P=P+Q;
Kgain=P/(P+R);
X_curr=X_curr+Kgain*(gyro_value-X_curr);
P=(1-Kgain)*P;
X_old=X_curr;
return (X_curr);
}


void CalcGyroOffset()
{
for(i=0;i<sample_size;i++)
{
gyro_value=SensorHTGyro(GyroScopePort);
gyro_sum+=gyro_value;
Wait(4);
}
gyro_avg=gyro_sum/sample_size;
offset=gyro_avg+0.5;
}

void ShowLCD()
{

TextOut(0,LCD_LINE1,"theta=");
NumOut(6*6,LCD_LINE1,theta-theta_bias);
Wait(20);
ClearScreen();
}


task main()
{
SetSensorHTGyro(GyroScopePort);

CalcGyroOffset();
prev_tick=CurrentTick();
while(true)
{
gyro_value=SensorHTGyro(GyroScopePort,offset);
f2=KalmanFilter(gyro_value)/GyroScale;
Wait(1);
theta=prev_theta+(f1+2*f2)*(CurrentTick()-prev_tick)/t_scale;
prev_theta=theta;
gyro_value=SensorHTGyro(GyroScopePort,offset);
f1=KalmanFilter(gyro_value)/GyroScale;
prev_tick=CurrentTick();
theta_bias=(1-aa)*theta_bias+aa*theta;
ShowLCD();
}
}

2011年5月23日 星期一

Kalman Filter 實現於NXT 感測器數據量測

接續前一個主題,我們期望Kalman Filter可以有效改善感測器對noise之敏感性。

理想的情況下,計算出陀螺儀初始的偏移量(bias)後,期望陀螺儀的感測元件讀值扣除偏移量之後,機身不動,理論上讀值應為某一個準位(例如直立時,讀值為0

然而,很不幸地,陀螺儀感測元件在靜止狀態時,仍有數據產生飄移的現象。

在程式中加入了Kalman Filter之後,果真使讀值飄移的現象大幅改善。

以下為經過陀螺儀靜止狀態5秒鐘內的數據讀取所呈現的結果:













上圖,綠色線代表未濾波時的陀螺儀讀取數據;
紅色線代表濾波的結果。

Perfect~

2011年5月22日 星期日

Kalman Filter 減低noise 的模擬試驗

使用時機:
當系統測量的過程,遇到隨機擾動的noise時,便無法真正測得真實的數據,為了減少擾動誤差,所以加入了卡爾門濾波器(Kalman Filter )。

以純量,說明卡爾門濾波器運作原理:

prior X(k|k-1)=AX(k-1)+BU(k)..................(1)
prior P(k|k-1)=AP(k-1|k-1)A'+Q=P(k-1|k-1)+Q...............(2)
其中:
1.W(k)為第k時刻的process state noise,與X獨立,且滿足Probability Normal Distribution
p(w)~N(0,Q)
2.我們假設A=1(認定第k時刻和第k-1時刻無變異)
3.假設P(k)是X(k)的covariance: P(k|k-1)以k-1時刻推測k時刻的prior值

在第k時刻,同時會有一個measurement value Z(k),與prior H(k)X(k)的Residual:
Residual =Z(k)-H(k)X(k|k-1)=Z(k)-X(k|k-1).................(3)
假設H=1(測量值與process state都是相同的單位,無物理轉換關係)

posteriori X(k|k)=X(k|k-1)+Kgain*(Residual).................(4)
1.其中Kgain是Kalman filter 增益
2.Kgain=P(k|k-1)H'(HP(k|k-1)H'+R)-1=P(k|k-1)/(P(k|k-1)+R), P(k)為(2)的值
3.R是measurement noise與Z獨立,且滿足Probability Normal Distribution
p(v)~N(0,R)
4.X(k|k)是後測的process state,推測第k時刻的最佳值

P(k|k)=(1-Kgain)*P(k|k-1)...................(5)

-------------------------------
程式:
clear all
clc
A=1;
B=1;
H=1;
U=0;
N=300;
desire=25*ones(1,N);

X=zeros(1,N);
measurement_noise=randn(1,N);
process_noise=randn(1,N);
Y=2^0.5*(measurement_noise)+desire; %NormalDistribution random value

% set initial state variable and its covariance value
X(1)=0;
P=1;
Q=1e-4; %set probability of process noise covariance P(w)~N(0,Q)
R=1e-1; %set probability of measurement noise covariance P(v)~N(0,R)

for k=2:N
X(k)=X(k-1);
P=P+Q;
KGain=P/(P+R);
X(k)=X(k)+KGain*(Y(k)-X(k));
P=(1-KGain)*P;
end
plot(X);
hold on
plot(desire,'r-');
plot(Y,'g*');
xlabel('time');
ylabel('temperature');
hold off

------------------------------------
[結果]





















其中
1.紅色水平線-期望值(用以觀察濾波後的值與之對照)
2.綠色點-未濾波時的數據(受到擾動很嚴重)
3.藍色線-濾波後的數據(很貼近期望值)

2011年5月3日 星期二

用Kalman filter 的技術來減低noise干擾

一篇經典的文獻,探討感測器偵測值易受周遭環境影響而產生white noise,應用Kalman filter 的技術來減低noise干擾,以提升感測器精準的量測。


參考文獻http://www.swarthmore.edu/NatSci/echeeve1/Ref/Kalman/ScalarKalman.html