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();
 }
}