Kalman Filter กับ Inertial Navigation – ไม่หลงทางแม้ไม่มี GPS (ตอนที่ 2)

การทำงานของ Kalman Filter ใช้แบบจำลองทางคณิตศาสตร์ของระบบในการประมาณสถานะของระบบล่วงหน้า โดยทั่วไปแล้วคุณลักษณะของระบบทางกายภาพ (Physical System) สามารถแสดงด้วยแบบจำลองทางคณิตศาสตร์แบบ State Space Representation ที่ประกอบด้วยสถานะของระบบ (State Variable) สัญญาณรบกวน (Noise) และ Input กับ Output ในรูปของสมการอนุพันธ์ (Differential Equation) ได้ ตามภาพ

โดยที่ x[n], u[n], w[n] และ y[n] ในรูปของ Vector ตัวแปร คือ สถานะ (State Variables), สัญญาณ Control Input, สัญญาณรบกวน (Noise) และ Output ของระบบ ส่วน A, B และ C ในรูปของ Matrix ค่าคงที่ คือ System Matrix, Input Matrix และ Output Matrix ตามลำดับ โดยที่ x[n+1] คือค่าสถานะที่เปลี่ยนแปลงไปเนื่องจากสถานะก่อนหน้า และ Input ที่ป้อนเข้าไปในระบบ รวมถึงสัญญาณรบกวนในระบบด้วย

อย่างไรก็ดี ค่าคงที่ใน System Matrix เป็นค่าที่ได้จากการจำลองคุณลักษณะของระบบส่วนสมการคณิตศาสตร์ ซึ่งอาจมีความคลาดเคลื่อนได้ นอกจากนี้การวัดค่า Output ของระบบ จำเป็นต้องใช้อุปกรณ์ในการตรวจวัด ซึ่งอาจมีความคลาดเคลื่อนในการตรวจวัดทำให้ไม่ได้ค่าที่แท้จริง โดยสามารถแสดงเป็น State Space Diagram ได้ดังนี้

โดย yv[n] คือค่า Output ที่วัดได้จากอุปกรณ์ตรวจวัด และ v[n] คือความคลาดเคลื่อนหรือสัญญาณรบกวนในอุปกรณ์ตรวจวัด

จากภาพดังกล่าว สามารถยุบรวม System Block และ Sensor Block เข้าด้วยกันเป็น Plant P ที่รับค่าจาก Input u[n] และสัญญาณรบกวน w[n] และ v[n] กับให้ค่า Output ที่วัดได้คือ yv[n] ซึ่งจากค่าดังกล่าวเราจะยังไม่สามารถแยกแยะได้ว่าค่าที่แท้จริงของระบบคืออะไร

Kalman Filter นำเอาค่า Input u[n] และ Measured Output yv[n] ไปคำนวณเพื่อหาค่าประมาณของ Output หรือ ye[n] ที่ใกล้เคียงกับค่าจริงมากที่สุดตามภาพ

Kalman Filter ใช้แบบจำลอง System Matrix, Input Matrix และ Output Matrix (A, B และ C) ของระบบ เพื่อใช้ประมาณค่าสถานะต่อไปของระบบ xe[n+1]  และค่าประมาณ Output ye[n] รวมทั้งใช้ค่า Measured Output yv[n] เพื่อนำไปคำนวณ Filter Gain ที่สามารถลด Error Covariance ของค่าประมาณ xe[n+1] ได้ดีที่สุด จากการคำนวณดังนี้

หลักการอีกอย่างหนึ่งของ Kalman Filter คือการคำนวณแบบทำซ้ำ ระหว่างการประมาณสถานะล่วงหน้า (Predict) กับการใช้ค่าที่วัดได้เพื่อนำมาปรับปรุงค่าประมาณปัจจุบัน (Correct) ด้วยการใช้ Filter Gain M ร่วมกับความแตกต่างระหว่างค่า Output ที่วัดได้กับค่าประมาณ (yv[n]-Cxe[n]) ซึ่งทำให้ Kalman Filter มีความแม่นยำสูงและทนต่อความคลาดเคลื่อนในระบบ โดยการคำนวณค่า Filter Gain M จะให้น้ำหนักกับค่าที่มีความน่าเชื่อถือมากกว่า (หรือมี Error Covariance ต่ำกว่า) ตัวอย่างเช่น หากค่า Measurement Error Covariance (R[n]) ต่ำ จะทำให้ค่า Measured Output yv[n] มีน้ำหนักมากกว่า แต่ในกรณีที่ค่า Process Error Covariance (P[n]) ต่ำ ก็จะทำให้ค่า Estimated State xe[n] มีน้ำหนักมากกว่า

การทดลองเปรียบเทียบค่า Measured Output yv[n] ที่ได้จากการตรวจวัด กับ Estimated Output ye[n] ที่ได้จาก Kalman Filter พบว่าค่า ye[n] มีค่าใกล้เคียงกับค่า y[n] หรือ Real Output โดยตรงจากระบบมากกว่า ซึ่งในกรณีของการใช้ Kalman Filter ในระบบ Inertial Navigation จะสามารถช่วยให้ค่าตำบลที่ที่มีความคลาดเคลื่อนจากตำบลที่จริงน้อยกว่าค่าที่วัดได้จากระบบโดยตรง

เมื่อเปรียบเทียบ Kalman Filter กับ Averaging Filter จะสังเกตได้ว่าผลที่ได้จาก Kalman Filter มีค่าใกล้เคียงกับค่า True Output มากกว่า โดยการใช้ Averaging Filter แสดงให้เห็นผลข้างเคียง คือ การ Smooth Out และ Time Delay ของผลที่ได้

Goten และ Trunks กำลังทำ Fusion

นอกจากการใช้ Kalman Filter ในการประมาณตำบลที่ในระบบ Inertial Navigation แล้ว ยังสามารถประยุกต์ใช้ Kalman Filter กับการทำ Data Fusion จากอุปกรณ์ตรวจวัดหลายประเภทได้ โดย Kalman Filter จะคำนวณค่า Filter Gain ที่ให้น้ำหนักกับข้อมูลจากอุปกรณ์ตรวจวัดที่มี Error Covariance ต่ำกว่า ซึ่งจะให้ผลการประมาณ Sensor Output ที่มีความแม่นยำสูง ซึ่งในกรณีของการหาตำบลที่ของเรือดำน้ำ สามารถใช้ Kalman Filter ในการประมวลข้อมูลจากอุปกรณ์ตรวจวัดต่างๆ ในเรือ เพื่อช่วยเสริมความแม่นยำให้กับการประมาณค่าตำบลที่ได้มากกว่าการใช้ข้อมูลจากระบบะ Inertial Navigation เพียงอย่างเดียว

statespace8

อ้างอิง

MATLAB Kalman Filtering Tutorial.

Welch and Bishop, “An Introduction to Kalman Filter.”

Antonio Moran, “Sensor Data Fusion Using Kalman Filters.”

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s