แนวคิดพื้นฐานและหลักการทำงานของ Kalman Filter ... - โรงเรียนนายเรือ

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๓๙d ( t)= x(t)(๓)d (t) คือคาที่ Stadimeter วัดได สมการที่ (๓) คือแบบจําลองทางคณิตศาสตรของระบบเรียกวาMeasurement Model เนื่องจากเปนสมการแสดงความสัมพันธระหวางสถานะของระบบกับคาที่“เซนเซอร” วัดได สมการที่ (๑) และ (๓) รวมกันคือแบบจําลองทางคณิตศาสตรของระบบที่สมบูรณ(System Model) นอกจากนี้ สมการที่ (๑) และ (๓) เปน System Model แบบ Deterministic กลาวคือตั้งอยูบนสมมุติฐานที่วา ความรูทุกอยางเกี่ยวกับระบบถูกตองแนนอน ๑๐๐ เปอรเซ็นต การใชสมมุติฐานดังกลาวมีขอจํากัดในชีวิตจริง เพราะ๑. ไมมีแบบจําลองทางคณิตศาสตรใดที่สมบูรณ ๑๐๐ เปอรเซ็นต สมมุติฐานที่วาเรือแลนดวยความเร็ว v( t)= F ⋅ x(t)ตามสมการที่ (๑) เปนเพียงการประมาณ ในชีวิตจริงมีตัวแปรเปนจํานวนมากที่มีผลตอความเร็วและการเคลื่อนที่ของเรือ ซึ่งไมสามารถนํามาเขียนเปนสมการไดอยางสมบูรณ (อายุการใชงานเครื่องยนต, อุณหภูมิ, การสึกหรอ, ประสิทธิภาพของเชื้อเพลิง ฯลฯ)๒. ไมมีเซนเซอรใดที่วัดคาไดสมบูรณ ๑๐๐ เปอรเซ็นต ในทางปฏิบัติเซนเซอรทุกชนิดมีความคลาดเคลื่อน (Measurement Noise) จะมากหรือนอยขึ้นอยูกับหลายปจจัย นอกจากนี้ ในชีวิตจริงมีสถานะของระบบหลายประเภทที่เซนเซอรไมสามารถวัดไดโดยตรง เชน ถาตองการรูตําบลที่บนโลกหลายคนอาจแนะนําใหใช GPS แตสิ่งที่เครื่องรับ GPS วัดไดจริง ๆ ไมใชตําแหนงแตเปนคลื่นสัญญาณวิทยุที่ถูกสงมาจากดาวเทียม การที่เครื่องรับ GPS สามารถบอกตําแหนงได ก็ดวยการคํานวณเวลาที่คลื่นใชเดินทางจากดาวเทียมมาถึงเครื่องรับ และแปลงเวลานี้เปนระยะหางระหวางดาวเทียมกับเครื่องรับเพื่อใชคํานวณหาตําแหนงอีกตอหนึ่ง ความพยายามที่จะแสดงความสัมพันธเหลานี้ในรูปของสมการทางคณิตศาสตรยอมมีความคลาดเคลื่อนมาเกี่ยวของ๓. ปจจัยภายนอกที่ไมสามารถควบคุมได จากตัวอยางขางตนปจจัยภายนอกที่มีผลตอความเร็วของเรือ เชน กระแสน้ํา, กระแสลม และคลื่น เปนตนดวยขอจํากัดของการวิเคราะหแบบ Deterministic ขางตน จึงนําไปสูการวิเคราะหแบบStochastic ซึ่งนําความไมแนนอน ขอมูลทางสถิติ และหลักการของความนาจะเปนมาพิจารณารวมดวยในตัวอยางนี้สามารถปรับปรุงและเขียนสมการที่ (๑) และ (๓) ไดใหมในรูปของ Continuous-TimeStochastic System Model เปน& (๔)x ( t)= Fx(t)+ w(t)= (๕)d ( t)x(t)+ v(t)

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๐w (t) และ v (t)เปนตัวแปรสุม (Random Variable) ใชเปนแบบจําลองความไมแนนอนในระบบ จากสมการที่ (๔) และ (๕) สามารถเขียน Discrete-Time System Model เพื่อหาระยะ x (t)ณเวลา t = ไดคือtkk= Φk−1 xk−1wk(๖)x +d x + vF tΦke ∆− 1= , ∆t= t k− tk −1, kk= (๗)kkx และ xk−1คือระยะหางระหวางเรือกับประภาคารที่เวลา tkและ t = t k− 1ตามลําดับ ดังนั้นจะเห็นไดวาสมการที่ (๖) ชวยใหสามารถหาสถานะของระบบที่เวลาt = t kไดจากขอมูลกอนหนา (สถานะของระบบที่เวลา t = t k− 1) แตมีคําถามวาจะใชคา wkและ vkคืออะไรเนื่องจากเปนตัวแปรสุมตามที่ไดกลาวแลว การเคลื่อนที่ของเรือมีความไมแนนอนในระดับหนึ่งเนื่องจากหลายปจจัย เชนกระแสน้ํา กระแสลม เปนตน สมมุติวาเราสามารถจําลองความไมแนนอนของการเคลื่อนที่ไดโดยกําหนดให w k= N(0,1)* และสมมุติใหความคลาดเคลื่อน (Measurement Noise) ของ Stadimeter คือv k= N(0,100) ภาพที่2b แสดงตัวอยางผลการจําลองการเคลื่อนที่ของเรือและคาที่ Stadimeter วัดไดเมื่อสุมตัวอยางทุก ๆ ๐.๕ วินาที ( = 0.5sec∆t ) เปนเวลา ๖๐ วินาที และ F = 0. 03จาก ภาพที่ ๒ ทําใหเกิดคําถามวา ในเมื่อเรารูความสัมพันธของสถานะของระบบกับตัวแปรอื่นๆรูคุณลักษณะทางสถิติของระบบและของเซนเซอร เราจะสามารถนําขอมูลทั้งหมดมาใชประกอบกับหลักการของความนาจะเปน ในลักษณะเกื้อกูลกันเพื่อหาคาประมาณที่ดีที่สุดของสถานะของระบบไดหรือไมอยางไร และคําตอบก็คือสามารถทําไดโดยการใช Kalman Filtert =*2X = N(µ , σ ) หมายความวา X เปนตัวแปรสุมแบบ Gaussian (หรือ Normal) มีคา Mean เทากับ2µ และ Variance เทากับ σ คุณสมบัติหนึ่งของตัวแปรสุมแบบ Gaussian คือ สามารถกําหนดกรอบของความมั่นใจไดวาประมาณ ๖๘%, ๙๕% และ ๙๙% ของคาที่ไดจากการสุมทั้งหมดจะอยูในชวงµ ± σ , µ ± 2σและ µ ± 3σตามลําดับ ดังแสดงในภาพที่ 2a กลาวโดยทั่วไป “Varianceเปนเหมือนเกณฑที่ใชกําหนดวาขอมูลมีความแมนยํามากนอยเพียงไร ยิ่ง Variance มีคาสูงความแมนยําจะยิ่งนอย”

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๑Stadimeter Noise Samples200-20µ±σµ±2σµ±3σ0 10 20 30 40 50 60Stadimeter Noise Samples v k= N(0,10)(a)ระยะจากประภาคาร x(t)806040200-20Sampling Period 0.5 secระยะ x(t) จริงของเรือคํานวณจากความเร็วระยะ x(t) ที่วัดไดดวย Stadimeter-400 10 20 30 40 50 60เวลา (วินาที)(b)ภาพที่ ๒ การจําลองการเคลื่อนที่ของเรือและคาที่วัดไดจาก Stadimeter๓ Kalman Filter Algorithmแมวารูปแบบมาตรฐานของ Kalman Filter Algorithm จะอยูในรูปของเมตริกซ เพื่อใหใชงานไดกับระบบที่มีสัญญาณเขา-ออกหลายสัญญาณ (Multiple Input Multiple Output System) และเพื่อใชประมาณสถานะของระบบหลายๆ สถานะ ในบทความนี้จะนําเสนอ Kalman Filter Algorithm ในรูปของScalar เพื่อใหงายตอการทําความเขาใจแนวคิดพื้นฐานและหลักการทํางานของ Kalman FilterKalman Filter คือสูตรทางคณิตศาสตรใชสําหรับหาคาประมาณที่ดีที่สุดของสถานะของระบบโดยนําขอมูลเกี่ยวกับความไมแนนอน เชน ความไมแนนอนของกลศาสตรของระบบ (SystemDynamics), ความคลาดเคลื่อนของเซนเซอร (Measurement Noise) มาประกอบการพิจารณาบนพื้นฐานของความนาจะเปนในลักษณะที่เกื้อกูลกันอยางดีที่สุด (Optimal) [๔, ๕] การใช Kalman Filterเริ่มตนดวยสมมุติฐานวาเราสามารถเขียนแบบจําลองทางคณิตศาสตรของระบบในรูปของ Discrete-TimeSystem Model ดวยสมการ

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๒k= Φk−1 xk−1wk(๘)x +zk= Hkxk+ vk(๙)สมการที่ (๘) และ (๙) คือ Discrete-Time Process Model และ Measurement Modelตามลําดับ wkและ vkคือ Zero-Mean Gaussian White Noise ซึ่งหมายความวา wkและ vkเปนตัวแปรสุมแบบ Gaussian มีคาเฉลี่ยเปนศูนย และกําหนดให Variance ของ w kและ vkมีคาเปน QkและRkตามลําดับ คําวา White หมายความวาคาของ wkและ vkไมเกี่ยวของกัน โดยสิ้นเชิง(Uncorrelated) (หรืออีกนัยหนึ่ง แมวาเราจะรูคาของ w หรือ v ณ เวลาหนึ่งๆ ก็ไมสามารถนํามาใชทํานายคาของ w หรือ v ณ เวลาอื่นๆ ได) ในทางคณิตศาสตรเราสามารถเขียนบรรยาย w และ v ไดดังนี้Mean ของ = E[ w] = 0w , Variance ของ w = E[ w w ]Mean ของ = E[] v = 0v , Variance ของ v = E[ v v ]kkjj⎧Qk, k = j= ⎨⎩ 0, k ≠ j⎧Rk, k = j= ⎨⎩ 0, k ≠ jCovariance ของ w และ v คือ E[ w v ] 0,for all k and jkj(๑๐)(๑๑)= (๑๒)E [] • หมายถึง Expected Valueการใชงาน Kalman Filter มีขั้นตอนดังนี้ ในขณะที่เซนเซอรยังไมสามารถวัดคาอะไรได เราสามารถประมาณคาสถานะของระบบ xkณ เวลา t = tkไดจากสมการx ˆ(๑๓)− ˆk= Φk−1xk−1เครื่องหมายลบเปนการระบุวา xˆkเปนคาประมาณโดยไมมีคาที่เซนเซอรวัดไดมาประกอบการพิจารณา การคาดการณไปในอนาคตยอมมีความไมแนนอน ใหความคลาดเคลื่อนในการคาดการณคือ−−ek= xk− xˆk(ความแตกตางระหวางสถานะจริงของระบบกับคาที่เราคาดการณจากการคํานวณ)−−เนื่องจากสมมุติฐานที่วา wkเปนตัวแปรสุมแบบ Gaussian เราสามารถพิสูจนไดวา xˆkและ ekจึงเปน−ตัวแปรสุมแบบ Gaussian ดวย โดยที่ ekมีคา Mean เปนศูนย และมี Variance คือ− 2 2[( ek) ] = Φk−1Pk1QkP +−k= E−เราสามารถใชสมการที่ (๑๓) และ (๑๔) เพื่อคาดการณคาของ x (t)และความนาเชื่อถือของการ−คาดการณ ( Pk) ไปเรื่อยๆ จนกระทั่งเซนเซอรสามารถวัดคาได เมื่อเซนเซอรวัดคาได Kalman Filterจะนําคาที่วัดไดมาประกอบการพิจารณาเพื่อหาคาประมาณสถานะของระบบ ตามสมการ(๑๔)

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๓โดยที่−( z − H xˆ)xˆ ˆ(๑๕)+ −k= xk+ Kk k k k−( P H2 + R ) −1−Kk= PkHk k k k(๑๖)เครื่องหมายบวกเปนการระบุวา xˆkเปนคาประมาณโดยไดนําคาที่เซนเซอรวัดไดมา−ประกอบการพิจารณา สมการที่ (๑๕) คือสมการที่ใช Update คา xˆkที่เราคาดการณไวลวงหนา (กอนที่−เซนเซอรจะวัดคาได) เมื่อพิจารณาจากสมการที่ (๑๕) และ (๙) แลวจะเห็นวา Hkxˆ kคือ สมการที่−ใชประมาณคาที่เซนเซอรควรวัดได ( ẑk) คํานวณโดยใชคาประมาณ xˆkKalman Filter จะทํางานโดย−หา “Residual” ( zk− Hkxˆ k) ซึ่งเปนผลตางระหวางคาที่เซนเซอรควรวัดไดกับคาที่วัดไดจริง นํามาให−น้ําหนักโดยการคูณดวย Kalman Gain Kkแลวนําผลที่ไดมาใชแกไขคา xˆkที่ไดคาดการณไวลวงหนา−ผูอานจะสังเกตไดวาคาที่นํามาใชแกไข xˆkขึ้นอยูกับขนาดของ Residual และ Kkกลาวโดยทั่วไป ถาผลตางระหวางคาที่คาดการณกับคาที่วัดไดจริงมีมากและขอมูลมีความนาเชื่อถือนอย−−คา Kalman Gain Kkที่คํานวณไดจะมีคาสูง ทําใหตองแกไข xˆkมาก ( Kk( zk− Hkxˆ k) มีคามาก)ในทางกลับกันถาผลตางระหวางคาที่คาดการณไวกับคาที่วัดไดจริงมีนอยและขอมูลมีความนาเชื่อถือ−−คา Kalman Gain Kkที่คํานวณไดจะมีคาต่ํา ทําใหแกไข xˆkเพียงเล็กนอย ( Kk( zk− Hkxˆ k) มีคานอย) โดยอัตโนมัติ การคํานวณคา Kalman Gain Kkตามสมการที่ (๑๖) เปนการคํานวณที่ไดนํา+ขอมูลทางสถิติของระบบมาพิจารณาแลวและเปนคา Gain ที่จะทําให Pkมีคาต่ําสุด จากมุมมองนี้ทําใหเปนที่ยอมรับวา Kalman Filter เปน “Optimal State Estimator”ในการหาคาประมาณโดยใชสมการที่ (๑๕) เราสามารถพิสูจนไดวา คาความคลาดเคลื่อนของ++การประมาณ ek= xk− xˆ k(หรืออีกนัยหนึ่งคือความแมนยําของคาที่คํานวณได) เปนตัวแปรแบบGaussian มี คา Mean เปนศูนย และมี Variance คือk+ 22 − 2[( ek) ] = ( 1 − KkHk) Pk+ KkRk+P = E(๑๗)สุดทายผูอานจะสังเกตไดวา Kalman Filter ทํางานในลักษณะของการ Predict (คาดการณ−ลวงหนาวาสถานะของระบบ xˆkนาจะเปนอยางไรโดยไมใชคาจากเซนเซอร) และการ Estimate (หา+คาประมาณ xˆkเมื่อเซนเซอรวัดคาได) ตอไปจะเปนการพิจารณาเกี่ยวกับสมมุติฐานที่ใชในการพัฒนาKalman Filter Algorithm๑. การตั้งสมมุติฐานวาkw และkv เปนตัวแปรสุมแบบ Gaussian สมเหตุสมผลเพราะสาเหตุสองประการ ประการแรกตามทฤษฎี Central Limit Theorem ที่กลาวไววาเมื่อนําตัวแปรสุมหลายตัว

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๔มารวมกันผลลัพธที่ไดจะเปนตัวแปรสุมแบบ Gaussian เนื่องจากสัญญาณรบกวน (Noise) ที่พบในชีวิตจริงมักมีที่มาจากแหลงกําเนิดหลายแหลงรวมกัน จึงสามารถประมาณไดวา Noise ที่พบในชีวิตจริงเปนตัวแปรสุมแบบ Gaussian สอดคลองกับ Central Limit Theorem [๔] ประการที่สอง โดยทั่วไปวิศวกรจะสามารถหาไดเพียงคา Mean และ Variance ของตัวแปรสุมใดๆ จากการทดลอง ซึ่งเพียงพอสําหรับใชบรรยาย Probability Density Function ของตัวแปรสุมแบบ Gaussian ไดอยางสมบูรณ (ถาเปนตัวแปรสุ มแบบอื่นจําเปนตองมีขอมูลมากกวานี้) การที่สามารถบรรยาย Probability Density Function ไดสมบูรณทําใหสูตรที่ใชใน Kalman Filter มีความสมบูรณทางคณิตศาสตร (Tractable)๒. การตั้งสมมุติฐานวา wkและ vkเปน White Noise สมเหตุสมผลแมวา White Noiseจะไมมีในชีวิตจริง (เพราะ Power Spectral Density † ของ White Noise ครอบคลุมทุกความถี่ตั้งแต− ∞ ถึง + ∞ ) เนื่องจากในทางปฏิบัติระบบทุกระบบจะสามารถตอบสนองไดในชวงความถี่ที่จํากัด(Limited Bandwidth) ซึ่งต่ํากวาชวงความถี่ของสัญญาณรบกวนที่พบในชีวิตจริงโดยทั่วไป ดังนั้นเมื่อมองจากมุมมองของระบบที่มี Bandwidth จํากัด White Noise จึงไมแตกตางจาก Wideband Noise ที่พบในชีวิตจริง ดังแสดงใน ภาพที่ ๓ นอกจากนี้การใชสมมุติฐาน White Noise ชวยลดความซับซอนของการพัฒนา Kalman Filter Algorithm และถาตองการ เราสามารถเพิ่มเติม Noise ที่มีความเกี่ยวเนื่องกันในเวลา (Correlated) ในแบบจําลองทางคณิตศาสตรไดดวยเทคนิคทางคณิตศาสตรWhite NoisePower Spectral Densityชวงความถี่ที่สนใจSystem BandwidthWideband NoiseFrequencyภาพที่ ๓ System Frequency Response†เราสามารถอางถึงกระบวนการสุม (Random Process) ได ๓ วิธี [๓] คือ ๑. ระบุ AutocorrelationFunction - R (τ ) , ๒. ระบุ Power Spectral Density Function - S ( jω)หรือ ๓. ระบุ Mean Squared2Value - E [ X ] วิธีที่ ๑. และ ๒. คือสิ่งเดียวกันเพราะ R (τ ) กับ S ( jω)เปน Fourier Transform Pairกลาวโดยทั่วไป Power Spectral Density Function คือการแยกกระบวนการสุมออกเปนองคประกอบยอยๆ ในรูปของสัญญาณที่ความถี่ตางๆ ดังนั้น “ยิ่งกระบวนการสุมประกอบดวยสัญญาณยอยที่ความถี่ยิ่งสูงเทาใด กระบวนการนั้นก็จะเปนกระบวนการที่เปลี่ยนแปลงตามเวลาเร็วมากขึ้นเทานั้น”

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๕๔ ผลการจําลองการทํางานของ Kalman Filterภาพที่ ๔ แสดงผลการจําลองการประมาณระยะหางระหวางเรือและประภาคารดวย KalmanFilter เมื่อใช Matlab M-file ดังแสดงใน ภาพที่ ๖ภาพที่ ๔ ผลการจําลองการประมาณระยะหางระหวางประภาคารและเรือดวย Kalman Filterจาก ภาพที่ ๔ จะเห็นไดชัดวา Kalman Filter สามารถประมาณสถานะของระบบคือ ระยะหางระหวางเรือและประภาคารไดอยูในเกณฑดี สังเกตจากคาประมาณมีคาใกลเคียงกับสถานะจริง และเรียบกวาคาที่วัดไดดวยเซนเซอร ที่เปนเชนนี้เพราะ Kalman Filter ใชความรูเกี่ยวกับ Dynamics ของระบบ,คุณลักษณะทางสถิติของระบบและเซนเซอร และคาที่วัดไดจากเซนเซอรมาประมวลผลประกอบกันบนหลักการของความนาจะเปน เพื่อหาคาประมาณของสถานะของระบบที่ดีที่สุดจากโปรแกรมใน ภาพที่ ๖ จะสังเกตไดวา Kalman Filter มีลักษณะเดนอยางหนึ่งคือเปนการประมวลผลแบบ Recursive หมายความวา ใชเฉพาะขอมูลจากเวลากอนหนา t = t k− 1เพียงคาเดียวเทานั้นเพื่อคํานวณสถานะที่เวลาปจจุบัน t = tkขอดีของการประมวลผลแบบ Recursive คือไมตองใชหนวยความจํามาก ตางกับการหาคาประมาณดวยวิธีอื่น เชน Simple Moving Average ซึ่งจะตองกําหนดขนาด Sample Size และเก็บขอมูลไวประมวลผลพรอมกันทีเดียว ทําใหตองใชหนวยความจํามาก โดยเฉพาะอยางยิ่งถา Sample Size มีขนาดใหญ

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๖ภาพที่ ๕ เปนการเปรียบเทียบการประมาณสถานะของระบบดวย Kalman Filter กับวิธีSimple Moving Average ที่ใชขนาด Sample Size เทากับ ๑๐ และ ๓๐ ผลที่แสดงใน ภาพที่ ๕ อาจทําใหหลายคนแปลกใจเพราะไมเปนไปตามที่ทราบโดยทั่วไปวายิ่งใช Sample Size ยิ่งมากจะยิ่งใหคาเฉลี่ยใกลเคียงกับความเปนจริง ดังจะเห็นไดชัดวา Simple Moving Average ขนาด ๓๐ ไมสามารถประมาณสถานะของระบบไดดีเทา Kalman Filter (ที่จริงมีความคลาดเคลื่อน (Divergence) คอนขางสูง)ที่เปนเชนนี้เพราะระบบมีการเปลี่ยนแปลงสถานะตามเวลา (Dynamics) ทําให Simple Moving Averageไมสามารถติดตามสถานะไดอยางแมนยํา ผลที่แสดงใน ภาพที่ ๕ เปนการยืนยันถึงความสามารถและจุดแข็งของ Kalman Filter ซึ่งเหมาะกับการประมาณสถานะของระบบ Dynamicsภาพที่ ๕ เปรียบเทียบประสิทธิภาพของ Kalman Filter กับ Simple Moving Average

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๗clc, close all, clear all% Initial analysisdt = 0.5, t = 0:dt:60; % Set sampling timeF = .03, phi = exp(F*dt); % State transition Constantwsigma = 1, vsigma = 10 % Process and measurement noise standard deviationu = 0; % Mean valuewk = wsigma*randn(1,length(t))+u; % Generate process noisevk = vsigma*randn(1,length(t))+u; % Generate stadimeter noisexe = zeros(size(wk)); z=xe;% Use xe as true states of the systemxe(1) = abs(wsigma*randn)+10*(1856/3600); % Set the initial condition to be 10 nautical mile/hourz(1) = xe(1)+vk(1);for k = 2:length(t)xe(k) = phi*xe(k-1)+wk(k);z(k) = xe(k)+vk(k);end% Plot stadimeter noise samplessigma1 = ones(size(t))*vsigma; sigma2 = 2*sigma1; sigma3 = 3*sigma1;subplot(2,1,1), plot(t,vk,'ro',t,sigma1,'k:',t,-sigma1,'k:',...t,sigma2,'k-.',t,-sigma2,'k-.',t,sigma3,'k--',t,-sigma3,'k--')% Plot system statesubplot(2,1,2), plot(t,xe,t,z,'o')% Perform Kalman Filteringx_minus = 10; P_minus = 10^2; % Initial conditionsPhik = phi; Qk = wsigma^2; Hk = 1; Rk = vsigma^2;for k = 1:length(t)% EstimateKk = P_minus*Hk*inv(P_minus*(Hk^2)+Rk);x_plus(k) = x_minus + Kk*(z(k)-Hk*x_minus);P_plus = ((1-Kk*Hk)^2)*P_minus+(Rk*Kk^2);% Predictx_minus = Phik*x_plus(k);P_minus = P_plus*Phik^2+Qk;end% Plot resultsfigure, plot(t,xe,t,z,'ro',t,x_plus,'bs')xlabel('เวลา (วินาที)'), ylabel('ระยะหางระหวางเรือและประภาคาร')legend('สถานะจริงของระบบ','คาที่วัดไดดวยเซ็นเซอร','คาที่ประมาณดวย Kalman Filter')๕ สรุปภาพที่ ๖ Matlab M-file สําหรับจําลองการประมาณระยะหางระหวางประภาคารและเรือบทความนี้ไดพยายามนําเสนอวา Kalman Filter คืออะไรและใชงานอยางไร โดยเนนแนวทางและตัวอยางที่ไมซับซอน เพื่อใหผูอานสามารถทําความเขาใจแนวคิดพื้นฐานที่เปนสาระสําคัญโดยงายมากกวาที่จะดึงใหผูอานตองศึกษาทฤษฎีทางคณิตศาสตรที่ซับซอนเกี่ยวกับ Kalman Filter จากตัวอยางผลการจําลอง การทํางานของ Kalman Filter จะเห็นไดวา Kalman Filter คือ สูตรทางคณิตศาสตรใชสําหรับประมาณสถานะของระบบที่เปลี่ยนแปลงตามเวลา (Dynamics) โดยนําหลักการทางสถิติและความนาจะเปนมาประยุกตใชประกอบการวิเคราะห บทความนี้ไดนําเสนอจุดแข็งของ Kalman Filter ในแงที่เปน Recursive Algorithm ทําใหประหยัดหนวยความจําในการประมวลผล และความสามารถในการ

ปที่ ๔ ฉบับที่ ๔ ตุลาคม – ธันวาคม ๒๕๔๗๔๘ติดตามประมาณคาสถานะของระบบ Dynamics เปรียบเทียบกับวิธี Simple Moving Average หวังเปนอยางยิ่งวาบทความนี้จะเปนจุดเริ่มตนและมีสวนชวยใหผูสนใจสามารถศึกษาเพิ่มเติมไดดวยตนเองและสามารถนํา Kalman Filter ไปประยุกตใชในชีวิตจริงในระดับสูงตอไปเอกสารอางอิง๑. Kalman, R.E. A new approach to linear filtering and prediction problems, Transaction ofthe ASME, Journal of Basic Engineering, March, 1960, p. 35-45.๒. Leonard, A. McGee and Stanley F. Schmidt.. Discovery of the Kalman Filter as apractical tool for aerospace and industry, NASA Technical Memorandum 86847,November, 1985.๓. Brown, Robert Grove and Patrick Y. C. Hwang. Introduction to random signals andapplied Kalman filtering. John Wiley & Sons., 1997.๔. Peter, S. Maybeck. Stochastic models, estimation, and control Vol.1, Academic PressInc., 1979.๕. Gelb, Arthur. Applied optimal estimation, The analytic sciences corporation, 1974.