Complementary Filter: จากอัตราหมุนไปเป็นมุมที่ผสานแล้ว

เริ่มตัวอย่างคำนวณ · กลับบทประยุกต์ IMU

อ่านต่อ บทมุมข้าม ±180° พร้อมตัวอย่าง เพื่ออ่านผลต่างมุมที่ข้ามขอบช่วง

วัตถุประสงค์และพื้นฐานที่ใช้

ฝึกอธิบายว่า gyro ช่วยทำนายมุมอย่างไร accelerometer ช่วยปรับมุมอย่างไร และเหตุใดน้ำหนักผสานต้องสัมพันธ์กับช่วงเวลา เริ่มด้วยเครื่องคิดเลข PC หรือกระดาษ ไม่ต้องมีบอร์ด อ่าน การคาลิเบรต IMU และ อัตราสุ่มตัวอย่าง ก่อนทำ

ภาพเสมือนจริงต้นบทสร้างด้วย AI แสดงบริบทการเรียน สมุดและอุปกรณ์ในภาพไม่ใช่ผลวัดหรือวงจรที่รับรอง ชุดนี้เป็นตัวเลขสมมติ หนึ่งแกน roll ไม่ใช่เฟิร์มแวร์บินหรือการจำลองท่าทางสามมิติครบระบบ

ผสานค่าที่มีความหมายและหน่วยเดียวกัน

gyro ให้ อัตราหมุน เช่น deg/s ส่วน accelerometer ให้ specific force เช่น g หรือ m/s² จึงไม่เฉลี่ยค่าดิบสองชนิดเข้าด้วยกัน ต้องแปลงข้อมูลให้อยู่ในรูปมุมภายใต้นิยามแกนที่ตรงกันก่อน ดูแนวคิด prediction และการให้น้ำหนักใน เอกสาร Complementary Filter ของ AHRS

สำหรับโจทย์ roll อย่างเดียวใช้ body FRD เดิม: X หน้า Y ขวา Z ลง มุมบวกตามกฎมือขวา ขณะวางระดับนิ่ง specific force เป็น [0, 0, −1] g และนิยาม acc_angle = atan2(−ay, −az) แล้วแปลง rad เป็น degree ตัวอย่าง ay = −0.5 g และ az = −√3/2 g ให้ +30° เมื่อไม่มีการเคลื่อนที่แทรก สูตรนี้จำกัดตามท่า/แกนของโจทย์ ไม่คัดลอกเครื่องหมายจาก library ที่ใช้คนละกรอบพิกัด

บทนี้เริ่มจาก acc_angle ที่ประกาศแล้ว ไม่ใช้ raw counts และไม่คำนวณ yaw จาก accelerometer การประมาณมุมจากแรงโน้มถ่วงต้องระวังแรงจากการเคลื่อนที่ อ่านสมมติฐานของ ADI AN-1057: inclination sensing และตรวจ กรอบพิกัด PX4 v1.16 ก่อนต่อกับ driver จริง

อ่านสองขั้นของสูตร

ใช้หน่วย degree, deg/s, second ทั้งโจทย์ ถ้าระบบจริงใช้ rad/s ต้องทำให้หน่วยตรงก่อนแทนสูตร

1. ทำนาย: prediction = previous_estimate + gyro_rate × dt โดย gyro_rate ถือคงที่แทนทั้งช่วง เป็น Euler integration แบบสอนหนึ่งแกน
2. ผสาน: estimate = alpha × prediction + (1 − alpha) × acc_angle ใช้ผลผสานรอบก่อนเป็น previous_estimate รอบถัดไป

prediction กับ acc_angle ต้องอยู่ในหน่วย degree และนิยามแกนเดียวกัน alpha ไม่มีหน่วย โจทย์กำหนด alpha = tau / (tau + dt) โดย tau และ dt เป็น second และ tau > 0, dt > 0 น้ำหนักสองส่วนรวมเป็นหนึ่ง แต่ไม่ได้ยืนยันว่าข้อมูลเข้าถูกต้อง ค่า alpha ใกล้หนึ่งให้น้ำหนัก prediction มาก; ใกล้ศูนย์ให้น้ำหนัก acc_angle มาก ไม่ใช่ความน่าจะเป็นว่าฝั่งใดถูก

กำหนด tau = 0.5 s เพื่อฝึกคำนวณ: dt = 0.01 s ให้ alpha = 50/51 ≈ 0.980392; dt = 0.02 s ให้ alpha = 25/26 ≈ 0.961538 ไม่ใช้เลข alpha เดิมโดยละเลย dt เมื่อช่วงเวลาเปลี่ยน สูตร alpha นี้เป็นรูปแบบที่เลือกสำหรับบทเรียน ไม่ใช่ API หรือค่าที่เหมาะกับทุก flight stack

คำนวณสามรอบและอ่านจุดที่ปรับมุม

สมมติฐาน: เริ่ม t = 0 ms, estimate = 0°, tau = 0.5 s, gyro_rate = 10 deg/s ทุกช่วง ไม่มี bias/clipping/quantization และค่ามุม accelerometer ถูกประกาศตามตาราง ใช้ timestamp ของ sample จาก clock เดียวกัน ไม่ใช่เวลาอ่าน USB โจทย์ไม่ได้ประกาศมุมจริง จึงไม่มี RMSE หรือข้อสรุปความแม่นยำ

เวลาและน้ำหนักของแต่ละรอบ:

Sample (ms)	dt (ms)	alpha
10	10	50/51
20	10	50/51
40	20	25/26

มุมที่ทำนายและมุมที่ผสานแล้ว:

Sample (ms)	ทำนาย (°)	มุม accel (°)	ผสาน (°)
10	0.100000	0.00	0.098039
20	0.198039	0.00	0.194156
40	0.394156	3.00	0.494381

รอบแรก: prediction = 0 + 10 × 0.01 = 0.1°; estimate = (50/51) × 0.1 + (1/51) × 0 = 0.098039° รอบสองต้องเริ่มจาก estimate นี้ ไม่เริ่มจากศูนย์ใหม่

รอบสามมี dt = 20 ms ใช้ alpha = 25/26 และ previous_estimate รอบสอง เมื่อ acc_angle เปลี่ยนเป็น 3° ผลผสานถูกดึงสูงกว่า prediction การเปลี่ยนนี้แสดงผลของข้อมูลเข้าเท่านั้น ไม่พิสูจน์ว่าโดรนเอียงจริงหรือ accelerometer ผิด เก็บค่าทศนิยมเต็มระหว่างคำนวณแล้วปัดเฉพาะค่าที่แสดง ตารางใช้ Euler ต่างจากชุด PC เดิมที่อินทิเกรต gyro แบบ trapezoid จึงไม่แทนผล RMSE เดิม

สร้างด้วย Matplotlib จากสูตรที่ประกาศ เส้นเชื่อมจุดมีไว้ให้อ่านลำดับ ไม่ใช่ waveform ต่อเนื่องที่วัดจริง เปิดกราฟและไดอะแกรมผสาน IMU ขนาดเต็ม

เวลา แรงแทรก และข้อจำกัดที่ต้องตรวจ

dt ต้องตรงช่วงของข้อมูลที่ใช้ ถ้า timestamp ไม่เพิ่ม ข้อมูลซ้ำ หรือคนละ clock ยังไม่เทียบเวลา ให้แยกเหตุผลและวิธีจัดการก่อนคำนวณ ไม่ใช้ dt = 0 หรือค่าติดลบแทน sample ใหม่ สำหรับช่องว่างเวลาที่ยาวขึ้น การแทน gyro_rate ค่าเดียวตลอดช่วงเป็นข้อสมมติเพิ่ม ไม่กู้คืนการเคลื่อนที่ที่ไม่ได้วัด

accelerometer ไม่แยกแรงจากการเคลื่อนที่ออกจากแรงโน้มถ่วงให้เอง การตรวจ norm ใกล้ g เป็นเพียงเงื่อนไขหนึ่ง ไม่ยืนยันว่ามุมใช้ได้ทุกครั้ง อย่าเพิ่มน้ำหนัก accelerometer เพื่อทำให้กราฟดูเรียบอย่างเดียว ต้องตรวจหน่วย แกน bias ความสด อัตราข้อมูล และสถานะการเคลื่อนที่

แบบจำลองนี้ไม่รวมมุมข้าม ±180°, pitch/yaw หรือ singularity ของ Euler angles การผสานแบบเชิงเส้นอาจผิดที่ขอบมุม และอัตราหมุน body สามแกนไม่เท่ากับอนุพันธ์ Euler angles ทุกท่า ระบบสามมิติใช้การแทนท่าทางและ estimator ที่เหมาะสม ศึกษา PX4 EKF v1.16 ต่อจากความหมายของข้อมูลเข้า ไม่ถือว่าเปลี่ยนเป็น EKF แล้วความผิดพลาดจะหายทั้งหมด

ขั้นตอนฝึกและคำแนะนำเบื้องต้น

1. อ่าน คู่มือชุด Complementary Filter แล้วทำสำเนา ใบวางแผนผสาน IMU ว่าง เขียนแกน หน่วย clock วิธี integration และค่าเริ่มต้น
2. ทำ โจทย์ผสานมุมสามรอบ ก่อนเปิด เฉลยและข้อจำกัด คำนวณ dt เป็น second ก่อนหา alpha และแยก prediction จาก estimate ไม่มีคะแนนใหม่ในแบบประเมินเดิม
3. เทียบกับ ชุดฝึก IMU บน PC เดิม อ่าน protocol และวิธี integration ของชุดนั้นก่อนเทียบผล ไม่แทนข้อมูลหรือเฉลยเดิมด้วยตารางสามรอบนี้
4. เขียนสิ่งที่โจทย์ยังไม่ยืนยัน เช่น ความแม่นยำกับมุมจริง latency และพฤติกรรมเมื่อแรงแทรก อ่าน การตรวจอายุข้อมูล เพื่อจัดการความสดก่อนนำผลไปใช้
5. เมื่อมีอุปกรณ์จึงต่อ คู่มือเตรียมบอร์ดจริง เก็บ raw/config/reference/อุณหภูมิ และชุดวัดอิสระ บทนี้ไม่มีผลวัด IMU หรือค่าจูนการบิน

คำแนะนำเริ่มต้น: เปลี่ยนทีละปัจจัย เก็บข้อมูลต้นทาง และอธิบายเหตุที่ผลต่างก่อนปรับ tau ภาพสวย ผลคำนวณตรง หรือเรียนจบหนึ่งบทไม่ได้ยืนยันการบินนิ่ง

ที่มาและตรวจไฟล์: สมการและแหล่งอ้างอิงของชุดผสาน · รายการแฮชชุดผสาน

กลับภาพรวม MCU · เลือกกิจกรรม MCU กับ Sim