Kalman Filter

              ตอนแรกเริ่มหัดใช้ Kalman filter นั้นไม่สามารถทำได้ เริ่มแรกผมใช้แบบจำลองทางคณิตศาสตร์ที่ได้จาก System identification ซึ่งอยู่ในรูปของระบบแบบต่อเนื่อง (Continuous system) แล้วก็ใช้แบบจำลองนี้นำไปหาค่าเกนและหา Plant ของ Kalman filter หลังจากนั้นก็นำไปทดลองจริงพบว่าผลที่ได้นั้นคนละเรื่องกันเลย

คือ เราสามารถวัดตำแหน่งได้ แต่เราต้องการประมาณความเร็วของมอเตอร์ ซึ่งมันไม่ได้อย่างที่เราคิดเลย แต่เมื่อเราค่อยๆคิด และลองนำแบบจำลองแปลงเป็น Discrete แล้ว ค่อยนำไปหา Kalman gain และ plant ของ Kalman พบว่าผลที่ได้นั้นเกือบเกือบจะเท่ากับของจริงที่วัดได้เลย จากการที่เราศึกษาเรื่อง Kalman filter จากที่ไม่เข้าใจเลยว่ามันทำงานอย่างไร จากการทดลองและศึกษา โดยใช้เวลาประมาณหนึ่งอาทิตย์ ผมก็ได้พบทางออกแล้วว่า Kalman filter นั้นสามารถทำงานได้ และทำงานได้อย่างมีประสิทธิภาพเป็นอย่างมาก โดยที่ไม่เปลือง memory ของเราเลย และผลที่ได้ก็เกือบเท่าของจริงที่วัดมาเลย

                ซึ่งสามารถเขียนขั้นตอนได้ดังนี้ เรามี Transfer function ของระบบเราก็แปลงให้เป็น State-space  โดยใช้คำสั่ง Gc=ss(A,B,C,D) ในโปรแกรม Matlab เมื่อเราได้แบบจำลองแบบ Continuous แล้วจากนั้นก็ใช้คำสั่ง Gd=c2d(Gc,Ts) ,Ts คือ Sampling time ของระบบ เราก็จะได้ปริภูมิสถานะของระบบที่เป็นแบบ discrete แล้วหลังจากนั้นแล้วก็หาค่าเกนของ Kalman filter โดยใช้คำสั่ง [kal,L,P,M]=kal(Gd,Q,R) ก็จะได้ Kalman gain M จากนั้นก็นำไปเขียน measurement update equation            

x[n|n]=x[n|n-1]+M(y[n]-x[n|n-1])

x[n|n-1] อ่านว่า ประมาณสเตท x ที่เวลา n โดยใช้ข้อมูลจากเวลา n-1

ซึ่งเรามีตัวแปรครบหมดและสามารถแทนเข้าไปสมการด้านบนครบหมด และต่อจากนั้นเราก็เขียนสมการ Time update

x[n+1|n]=Ax[n]+Bu[n]

 แล้วก็นำสมการนี้ไปเขียนในโปรแกรมต่างๆต่อไป เช่น Labview/Micro controller เป็นอันเสร็จสิ้นขบวนการประมาณสเตทโดยใช้ discrete Kalman Filter แบบ Linear

Inverted pendulum state feedback control