Balancing robot with Kalman filter

จากครั้งที่แล้วหามุมการเอียงของหุ่นย์โดยใช้วิธี complementary filter พบว่าเมื่อหุ่นยนต์ run ไปนานๆแล้วมุมที่อ่านได้จาก sensor มีความผิดพลาดเกิดขึ้นเนื่องจากสัญญาณที่ได้เกิดการ drift เกิดขึ้นที่ gyro sensor ดังนั้นผมเลยเปลี่ยนวิธีในการหามุมเอียงใหม่โดยใช้วิธี sensor fusion kalman filter โดยมีขั้นตอนดังนี้ เริ่มจากการเขียน state-space ของ Predict model เสียก่อน  เรานิยามว่ามุมที่ได้จาก gyro ก็คือผลของการ Integrate สัญญาณจาก gyro นั้นเองและเรายังสามารถนิยาม bias ของสัญญาณขึ้นมาอีกตัวหนึ่งเพื่อที่จะทำการ tracking ค่า bias ให้มีค่าคงที่

หาสมการ  Kalman Predict

Kalman predict equation

x_dot=Ax+Bu
P=APA'+Q;
//***********************************************************************************//

theta_dot=(gyro-bias)    ...........................................................................(1)
bias_dot=0

จากสมการ (1) เรานิยาม state variable ดังนี้ x=[theta;bias]
จากนั้นให้เราหา jacobian matrix ก็คือการทำ partial derivative สมการ (1) เทียบกับ [theta;bias] จะได้ Matrix A,B ดังนี้

A=[0 -1;0 0],  B=[1;0]

เมื่อนำมาเขียนให้อยู่ในรูปของ state-space เราจะได้ดังนี้ จาก
x_dot=A*x+B*u

x_dot=[0 -1;0 0]*x+[1;0]*gyro   ...............................(2)


จะเห็นได้ว่า (2) ยังเป็นสมการที่อยู่ในรูปของระบบแบบ continuous  อยู่แต่ในทางปฎิบัติจริงส่วนมากแล้วเราจะใช้พวก MCU ดังนั้น เราจึง Transform ไปสู่ Discrete model เราจะได้
x_dot=(x(n+1)-x(n))/dt=[0 -1;0 0]*x+[1;0]*gyro;


x1_(n+1)=x1(n)+dt*(gyro-x2(n)) .............................................(3)
x2(n+1)=x2(n)

Ad=[1 -dt;0 1];

Bd=[dt;0];

gyro ก็คือค่า ที่อ่านจาก sensor
x1 คือ theta
x2 คือ bias
dt คือ sampling time

จากนั้นจะคำนวนหา Covariance matrix P จากสมการ
P(n+1)=Ad*P(n)*Ad'+Q    ..........................................(4)

โดยที่
Q=[Q_accelerometer 0;0 Q_gyro];

เมื่อคูณ  Matrix แล้วได้
P(n+1)=[P00 - dt P10 - (P01 - dt P11) dt + Q_accelerometer , P01 - dt P11;
               P10 -dt P11,  P11 + Q_gyro];

P00(n+1)=P00 - dt P10 - (P01 - dt P11) dt + Q_accelerometer ;   .......................(5)
P01(n+1)=P01 - dt P11;
P10(n+1)=P10 -dt P11;
P11(n+1)= P11 + Q_gyro;



จากนั้นพิจรณา Measurement Update
Measurement Update equation:
K=PH'inv(HPH'+R).........................................(6)
P=(I-KH)P......................................................(7)
x=x+K(y-Hx)...................................................(8)

H คือ measure output matrix
y คือ measure sensor
R measurement noise matrix

H=[1 0]; เนื่องจากเราวัดมุมได้อย่างเดียวนั้นคือ accelerometer
P=[P00 P01;P10 P11]

คูณ (6) ได้


K1=P00/(P00+R);
K2=P10/(P00+R);

คูณ (8) ได้


x1=x1+K1*(y-x1);
x2=x2+K2*(y-x2);

คูณ (7) ได้


P00=(1-K1)*P00;
P01=(1-K1)*P01;
P10=-K2*P00+P10;
P11=-K2*P01+P11;

เป็นอันเรียบร้อยในการทำ Kalman filter sensor fusion

จากนั้นเราก็จะนำสมการด้านบนทั้งหมดมาเขียนโปรมแกรมและโหลดลงใน Micro-conroller (CCS Compiler)

***********************************  CODE ****************************************
#include <18f458.h>
#device adc=10
#fuses H4, NOWDT, NOPROTECT, BROWNOUT, PUT, NOLVP
#use delay(clock=40000000)
#define  TxD         PIN_C6
#define  RxD         PIN_C7
#use rs232(baud=115200, xmit=TxD,rcv=RxD)
#include
#include
#include
#include


#use rtos(timer=0,minor_cycle=10ms)




float x[2][1]={{0.47},{0.0}};
float u[1][1];
float P[2][2]={{.01*.01*0.00000135 0 },{0}};
float K[2][1];
float x1=0.4656;



 float Q_roll=0.00000135,Q_roll_bias=.183;
const float R=0.028;
float dt=0.010;
 float fc=1.765;


float roll();
float qm();



float gyro;
float qm(){

set_ADC_channel(0);
delay_cycles(60);
gyro=(float)read_adc()*1/1023;
return(gyro);
}



float ax;
float roll(){

set_ADC_channel(2);
delay_cycles(60);
ax=atan((float)read_adc()*1/1023);
return(ax);
}





// ****************************** LPF ****************************************//
void LPF(){

x1+=dt*(-2*pi*fc*x1+2*pi*fc*ax);




}

float fhc=0.01;
float gyro1_old=0.34,tou,a,y=0;

void HPF(){
tou=1/(2*pi*fhc);
a=tou/(tou+dt);
y=a*y+a*(gyro-gyro1_old);



gyro1_old=gyro;

}

// ************************* KALMAN PREDICT **********************************//
void Kalman_Predict(float qm){
HPF();
   u[0][0]=y;
 
   x[0][0]=x[0][0]+dt*u[0][0]-dt*x[1][0];
   x[1][0]=x[1][0];
 
/* P=A*P*P'+Q */
 
P[0][0]=P[0][0]-dt*P[1][0]-(P[0][1]-dt*P[1][1])*dt+Q_roll;
P[0][1]=P[0][1]-dt*P[1][1];
P[1][0]=P[1][0]-dt*P[1][1];
P[1][1]=P[1][1]+Q_roll_bias;
 
}

//************************ KALMAN MEASURE ************************************//

void Kalman_Measurement(float roll){
LPF();

K[0][0]=P[0][0]/(P[0][0]+R);
K[1][0]=P[1][0]/(P[0][0]+R);

x[0][0]+=K[0][0]*(x1-x[0][0]);
x[1][0]+=K[1][0]*(x1-x[0][0]);

P[0][0]=(1-K[0][0])*P[0][0];
P[0][1]=(1-K[0][0])*P[0][1];
P[1][0]=-K[1][0]*P[0][0]+P[1][0];
P[1][1]=-K[1][0]*P[0][1]+P[1][1];

}



//****************************************************************************//

#task(rate=10ms,max=10ms)
void MAIN_LOOP(){
 
   Kalman_Predict(qm());
   Kalman_measurement(roll());


            }

#task(rate=10ms,max=10ms)
void print(){

printf("%.4f %.4f %.4f %.4f\r\n",ax,x[0][0],K[0][0],K[1][0]);
}


void main(){
    setup_port_a(0x07);
    setup_adc(adc_clock_internal);
    enable_interrupts(GLOBAL);

while(TRUE)

    {
           rtos_run();

     }

}


สนใจติดต่อ : sarayuthassa@gmail.com
                  083-4391356