Mô hình điều khiển Quadrocopter

Lôøi caûm ôn Kính göûi Thaày TS. Chung Taán Laâm ! Em laø Phan Leâ Sôn, sinh vieân naêm 4, ngaønh Cô Ñieän Töû , Tröôøng Ñaïi Hoïc Baùch Khoa Tp.HCM. Hoïc kyø naøy, em raát vinh döï ñöôïc thöïc hieän ñeà taøi cuûa ñoà aùn moân hoïc Thieát Keá Heä Thoáng Cô Ñieän Töû   döôùi söï höôùng daãn taän tình cuûa Thaày. Chính söï nhieät tình chæ daãn cuûa Thaày ñaõ giuùp em hoaøn thaønh ñoà aùn khaù toát, ñaït ñöôïc nhöõng yeâu caàu cô baûn maø ñeà taøi ñaët ra. Vaø cuõng qua ñoù, em ñaõ hoïc hoûi, tieáp caän ñöôïc nhieàu vaán ñeà thöïc teá hôn, boå sung nhöõng “khe hôû” kieán thöùc do vieäc hoïc thieân veà lyù thuyeát ñeå laïi. Em xin chaân thaønh caûm ôn Thaày. Cuõng qua ñaây, em xin göûi lôøi caûm ôn chaân thaønh cuûa mình ñeán gia ñình, baïn beø –nhöõng ngöôøi ñaõ ñoäng vieân, quan taâm heát möïc ñeán em trong suoát quaù trình thöïc hieän ñoà aùn. Ñoù laø nhöõng ñoäng löïc thuùc ñaåy em coá gaéng hôn, quyeát taâm hôn, ñeå cuoái cuøng em coù ñöôïc keát quaû nhö ngaøy hoâm nay. Em xin göûi lôøi caûm ôn ñeán taát caû. BÁO CÁO MÔ HÌNH ĐIỀU KHIỂN QUADROCOPTER Nội dung 1.   Giới thiệu về QuadroCopter 2.   Cấu tạo của QuadroCopter 3.   Nguyên lý hoạt động của QuadroCopter 4.   Xây dựng mô hình QuadroCopter trong VisualNastran 5.   Xây dựng mô hình điều khiển QuadroCopter trong Simulink của MatLab 6.   Lyù thuyeát veà boä loïc Kalman 7.   ÖÙng duïng boä loïc Kalman vaøo ño caùc goùc nghieâng cuûa maùy bay 8.   Höôùng phaùt trieån cuûa ñeà taøi . 9.   Phuï Luïc I. GIỚI THIỆU VỀ QUADROCOPTER: Quadrocopter là một loại máy bay có 4 cánh cùng nằm trong một mặt phẳng và đối xứng qua tâm của máy bay. Nhờ có 4 cánh nên loại máy bay này dễ điều khiển hơn và có khả năng giữ thăng bằng tốt hơn . Loại máy bay này có rất nhiều ứng dụng thực tế, nó có thể có kích cỡ lớn để chở người và hàng hoá và cũng có thể có kích cỡ nhỏ rất năng động có thể hoạt động ngay cả trong nhà . Loại máy bay kích cỡ bay trong nhà hiện nay đang phát triển rất rầm rộ , nhiều sinh viên của Đức và Mỹ đã chế tạo loại máy bay này với nhiều hình dáng và tính năng khác nhau để nghiên cứu . Trong phần đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopter hoạt động trong nhà . Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềm MatLab kết hợp với Visual Nastran . Sau đó nhóm đã thiết kế phần cứng máy bay và hoàn thành một số modul nhỏ của công việc điều khiển máy bay . Trong đồ án này có đưa vào modul đo góc nhiêng của máy bay bằng cảm biến gyro và cảm biến gia tốc dài được ước lượng bằng bộ lọc Kalman . Máy bay Quadrocopter II. CẤU TẠO CỦA QUADROCOPTER : - Thân : thường có hình dáng giống dấu chữ thập , mỗi cánh của chữ thập đỡ một cánh của máy bay. - Cánh : có 4 cánh giống hệt nhau đặt trên 4 góc của thân , 2 cánh đối diện thì quay cùng chiều nhau và ngược chiều với 2 cánh đối diện còn lại . Sở dĩ máy bay có cấu tạo như vậy để làm hạn chế sự quay của thân khi cánh quay. - Bộ điều khiển : bao gồm 2 phần , 1 phần đặt trên máy bay , một phần nằm ở dưới đất . + Phần đặt trên máy bay : bao gồm mạch công suất điều khiển 4 động cơ , mạch truyền sóng để kết nối với bộ điều khiển ở dưới đất , mạch chính dùng để điều khiển chuyển động của động cơ ( có thể bao gồm cả 2 phần là bằng tay và tự động ) .  Để điều khiển máy bay theo chế độ tự động ta còn phải có thêm các cảm biến như cảm biến gyro để đo độ nghiêng, cảm biển aùp suaát để đo độ cao , cảm biển gia tốc để biết hướng chuyển động , camera xử lý ảnh có nhiệm vụ theo dõi các đối tượng ở dưới đất . . . + Phần dưới đất : bao gồm bộ điều khiển bằng tay có mạch thu phát tín hiệu sóng vô tuyến, mạch kết nối với máy tính hoặc là mạch giao tiếp với người sử dụng để họ có thể thay đổi các thông số và các chế độ điều khiển . . . - Động cơ và bộ driver của động cơ : nếu máy bay chỉ hoạt động bay thông thường  thì chỉ cẩn bốn động cơ dùng cho 4 cánh quạt là đủ. Hình 2: Moâ hình QuadroCopter III. NGUYEÂN LYÙ HOAÏT ÑOÄNG CUÛA QUADROCOPTER: Ñeå ñieàu khieån ñöôïc hoaït ñoäng cuûa maùy bay ta phaûi ñieàu khieån tyû leå toác ñoä töông öùng giöõa 4 caùnh quaït . Sau ñaây laø caùch ñieàu khieån cuï theå: Bay leân: Boán caùnh quay vôùi vaän toác ñeàu nhau gioáng nhö hình veõ. ÔÛ treân hình chuùng ta thaáy 2 caùnh ñoái dieän cuûa maùy bay quay cuøng chieàu vôùi nhau vaø ngöôïc vôùi 2 caùnh coøn laïi . Ñieàu naøy raát quan troïng vì nhö vaäy noù seõ khoâng laøm maùy bay xoay quanh truïc vuoâng goùc vôùi maët phaúng maùy bay, theo nguyeân lyù baûo toaøn momanh ñoäng löôïng thì 2 caëp caùnh quay ngöôïc nhau seõ taïo ra caëp momanh ñoäng löôïng ñoái nghòch trieät tieâu laãn nhau. Hình 3 : Ñieàu khieån maùy bay bay leân Bay theo moät höôùng : - Giöõ nguyeân toác ñoä 2 caùnh theo höôùng khoâng dòch chuyeån . - Taêng toác ñoä caùnh naèm ôû goác vector theo phöông dòch chuyeån . - Giaûm toác ñoä caùnh naèm ôû ñænh vector theo phöông dòch chuyeån . Hình 4 : Ñieàu khieån maùy bay bay theo 1 höôùng Khi chuùng ta giöõ nguyeân toác ñoä 2 caùnh theo phöông khoâng dòch chuyeån va taêng, giaûm toác ñoä 2 caùnh theo phöông dòch chuyeån  khi ñoù chuùng ta seõ taïo cho maùy bay moät goùc ngieâng theo phöông dòch chuyeån . Chính nhôø goùc nghieâng naøy maø löïc naâng cuûa caùnh quaït khoâng coøn naèm theo phöông thaúng ñöùng , vaø toàn taïi thaønh phaàn löïc höôùng theo phöông chuyeån ñoäng . Chính thaønh phaàn löïc naøy ñaõ laøm maùy bay bay theo phöông ngang . Hình 5 : Löïc taùc duïng leân thaân maùy bay khi noù bò nghieâng Neáu chuùng ta muoán maùy bay bay theo phöông naøo cuûa thaân thì chuùng ta aùp duïng nguyeân taéc ñieàu khieån treân vaøo phöông ñoù . Xoay voøng quanh truïc vuoâng goùc vôùi maët phaúng maùy bay: - Khoâng maát tính toång quaùt , ta giaû söû caàn ñieàu khieån maùy bay xoay ngöôïc chieàu kim ñoàng hoà nhö hình veõ. Ta phaûi taêng vaän toác cuûa 2 caùnh ñoái dieän maø taêng  momanh ñoäng löôïng khieán maùy bay xoay ngöôïc chieàu kim ñoâng hoà vaø giaûm vaän toác 2 caùnh coøn laïi ñeå giaûm momanh ñoäng löôïng theo phöông ngöôïc vôùi chieàu xoay mong muoán cuûa maùy bay . Hình 6 : Ñieàu khieån maùy bay bay voøng quanh truïc vuoâng goùc vôùi maët phaúng maùy bay. IV. XAÂY DÖÏNG MO HÌNH QUADRO TRONG VISUAL NASTRAN : Phaàn meàm Visual Nastran laø phaàn meàm giuùp baïn taïo ñöôïc moâi tröôøng vaät lyù gaàn gioáng vôùi thöïc teá ñeå moâ phoûng hoaït ñoäng cuûa robot . Vôùi vieäc duøng phaàn meàm Visual Nastran keát hôïp vôùi Simulink cuûa Matlab giuùp ta coù moät moâi tröôøng moâ phoûng gaàn vôùi thöïc teá nhaát. Trong Visual Nastran caùc yeáu toá troïng löôïng , vaän toác , gia toác , momanh quaùn tính ñeàu ñöôïc xeùt ñeán . Ban ñaàu ta caàn phaûi xaây döïng moâ hình cô khí cho maùy bay tröôùc , vieäc xaây döïng moâ hình naøy ñöôïc thöïc hieän baèng phaàn meàm Pro/Engineer . Hình 7: Moâ hình cô khí cuûa maùy bay ñöôïc xaây döïng baèng phaàn meàm Pro/Engineer . Moâ hình treân laø moâ hình caûi tieán cuûa maùy bay maø nhoùm ñaõ xaây döïng sau nhaèm ñaùp öùng caùc yeâu caàu thöïc teá, coøn döôùi ñaây laø moâ hình maø nhoùm duøng ñeå moâ phoûng trong phaàn meàm Visual Nastran . Giöõa thaân maùy bay vaø 4 caùnh quaït ñöôïc gaén 4 khôùp quay . Khi caùnh quaït quay ta xaây döïng haøm truyeàn giöõa vaän toác caùnh quaït vaø löïc naâng cuûa noù . Vieäc tính toaùn haøm truyeàn naøy ñöôïc thöïc hieän trong moâi tröôøng MatLab , sau khi tính ñöôïc löïc naâng , ta ñöa löïc naøy vaøo trong moâi tröông Visual Nastran ( do trong Nastran khoâng coù chöùc naêng taïo löïc naâng khi caùnh quaït quay ) . Vieäc ñöa löïc vaøo 4 caùnh quaït chæ ñöôïc thöïc hieän vôùi ñoä chính xaùc töông ñoái , nghóa laø caùc löïc naøy khoâng ñoái xöùng nhau hoaøn toaøn . Laøm nhö vaäy ta coù theå kieåm tra ñöôïc ñaùp öùng cuûa luaät ñieàu khieån , bôûi vì khi maùy bay hoaït ñoäng trong moâi tröôøng thöïc teá thì caùc löïc naâng naøy cuõng khoâng hoaøn toaøn ñoái xöùng . Hình 8 : Moâ hình maùy bay trong Visual Nastran Trong caùc cöûa soå giao dieän cuûa Visual Nastran ta cho hieän leân caùc cöûa soå thoâng baùo löïc taùc duïng leân 4 caùnh maùy bay, goùc nghieâng theo 2 phöông x vaø y . V. XAÂY DÖÏNG MO HÌNH ÑIEÀU KHIEÅN CUÛA QUADROCOPTER TRONG SIMULINK CUÛA MATLAB : Nhiệm vụ của phaàn naøy là xây dựng một mô hình điều khiển máy bay QuadroCopter . Nếu ta muốn điều khiển hướng di chuyển của QuadroCopter thì trước tiên ta phải điểu khiển được độ nghiêng của máy bay theo 2 phương x , y ( oxy là mặt phẳng song song với mặt đất ). X Y Hình vẽ ký hiệu các cánh của QuadroCopter Nếu ta muốn cho máy bay nghiêng theo phương X thì ta phải thay đổi tỷ lệ vận tốc của cánh A và cánh C ( sự thay đổi vận tốc tương ứng với sự thay đổi lực nâng và lực vuông góc với trục ) . Tương tự nếu ta muốn thay đổi góc nghiêng theo phương Y thì ta phải thay đổi tỷ lệ vận tốc của 2 cánh B và D . Công thức liên hệ giữa lực nâng F ( vuông góc với mặt phẳng chứa cánh quạt ) và vận tốc gốc  ω của máy bay là : F = k.ω2 với k là hằng số phụ thuộc vào môi trường và cấu tạo của cánh máy bay . Mô hình điều khiển được xây dựng trên môi trường Simulink của MatLab kết hợp với VisualNastran . Dưới đây là mô hình điều khiển độ nghiêng của máy bay : Sơ đồ điều khiển trên bao gồm 3 phần : - Phần có khối Controller dùng để điều khiển độ nghiêng theo phương X . - Phần có khối Controller1 dùng để điều khiển độ nghiêng theo phương Y . - Phaàn coù khoái controller2 duøng ñeå ñieàu khieån ñoä cao theo phöông Z . Do 2 khối  ñaàu tương tự như nhau nên ta chỉ cần chỉ ra các khối con trong 1 khối là đủ: - Khối Step :  chung cho cả 2 khối lớn , đây là khối tạo ra giá trị tham khảo về độ nghiêng của máy bay theo 2 phương X, Y Giá trị tham khảo của góc nghiêng được cho như đồ thị sau : - Khối PID : Kp = 30 , KI = 15 , KD = 25 - Khối : Dùng để lượng tử hoá tín hiệu đầu vào và giữ nó trong  một chu  kỳ tính toán . -  Khối hàm truyền của động cơ : tín hiệu đầu vào là điện áp , tín hiệu đầu ra là vận tốc góc . - Khối bão hoà và khối giới hạn mức tăng  nhằm giữ cho tín hiệu đầu ra nằm trong khoảng giới hạn vật lý cho phép của mô hình . - Khối tính toán : với đầu vào là độ thay đổi vận tốc gốc và đầu ra là sự phân chia sự thay đổi giá trị đầu vào của vận tốc gốc cho 2 cánh . Nội dung trong khối này như sau: function y = fcn(u) % This block supports an embeddable subset of the MATLAB language. % See the help menu for details. y1= (u+sqrt(16-u*u))/2; y2= (-u+sqrt(16-u*u))/2 ; y = [y1 y2]; - Khối hàm truyền từ vận tốc gốc sang lực vuông góc với cánh : - Khối VisualNastran  : Khối này gồm 4 đầu vào đó là lực nâng của 4 cánh quạt . Đầu ra là độ nghiêng của máy bay theo phương X và Y . Nếu muốn có cánh quạt quay để hình ảnh mô phỏng trực quan gần với thực tế hơn ta có thể thêm vào 4 đầu vào vận tốc cho khối VisualNastran . YÙ töôûng xaây döïng khoái naøy laø töø goùc nghieâng ño veà ta so saùnh vôùi giaù trò tham khaûo ñeå ñöa ra sai soá goùc nghieâng , töø sai soá goùc nghieâng ta ñöa vaøo khoái PID tính ra ñöôïc möùc vol cheânh leäch caàn ñöa vaøo ñeå ñieàu khieån 2 caùnh quaït . Töø möùc Vol naøy , sau khi qua haøm truyeàn cuûa ñoäng cô ta ñöôïc möùc cheânh leäch vaän toác cuûa 2 caùnh quaït . Sau ñoù ta ñöa ñoä cheânh leäch naøy vaøo khoái chia vaän toác cho moãi ñoäng cô sao cho hôïp lyù nhaát. Sau đây là kết quả mô phỏng được thể hiện trên VisualNastran : Đồ thị trên biểu diễn góc nghiêng của máy bay theo phương X và Y  theo thời gian ( giá trị max và min của đồ thị góc trên là từ - 90 0 đến 90 0 ) . Trong đồ thị ta thấy trong 5 s đầu góc tương ứng theo 2 phương hướng tới trạng thái ổn định ( 00 )  và sau 5 s thì góc nghiêng chuyển sang 180 . Để thấy được kết quả của luật điều khiển ta so sánh đồi thị trên với đồ thị giá trị tham khảo của khối Step đã được đưa ra trong phần khối Step .  Kết quả mô phỏng bằng hình ảnh cho ta thấy ban đầu ( trong 5s đầu ) 2 góc của máy bay là 00 tương ứng với việc giữ thăng bằng trên không của máy bay . Và sau 5s góc điều khiển theo 2 phương là 180 nên ta thấy được đáp ứng của máy bay là thay đổi hướng ngay lập tức ứng với phương nghiêng điều khiển. Khoái ñieàu khieån ñoä cao : Söï khaùc bieät cuûa khoái ñieàu khieån ñoä cao so vôùi 2 khoái kia laø ôû choã , trong moãi khoái ñieàu khieån goùc nghieâng chæ taùc ñoäng ñeán 2 caùnh quaït töông öùng vôùi goùc nghieâng ñoù. Coøn trong khoái ñieàu khieån ñoä cao thì taùc ñoäng 1 luùc leân 4 caùnh quaït . Cuï theå nhö sau, sau khi ñoïc giaù trò ñoä cao veà ta so saùnh vôùi giaù trò tham khaûo thì thu ñöôïc sai soá ñoä cao . Sai soá naøy ñöôïc ñöa qua khoái PID ñeå tính ra toång möùc Vol caàn thay ñoåi . Giaù trò naøy laïi laø ñaàu vaøo cuûa khoái moâ hình hoaù ñoäng cô , ñaàu ra cuûa khoái moâ hình hoaù ñoäng cô chính laø vaän toác caàn thay ñoåi . Vaän toác naøy ñöôïc chia ñeàu cho 4 caùnh quaït ñeå ñaûm baûo khoâng gaây aûnh höôûng nhieàu tôùi ñoä nghieâng . Ñeå vieäc moâ phoûng ñöôïc tröïc quan hôn vaø deã söû duïng hôn nhoùm ñaõ thieât keá giao dieän vôùi ngöôøi duøng trong moâi tröôøng Matlab : Hình veõ giao dieän ñieàu khieån maùy bay moâ hình Giao dieän coù caùc thanh tröôït vaø caùc Edit box ñeå ngöôøi duøng coù theå nhaäp giaù trò ñieàu khieån vaøo maùy bay hoaëc quan saùt caùc giaù trò ño ñöôïc töø maùy bay . Caùc thanh tröôït X , Y laø ñeå ñieàu khieån goùc nghieâng theo phöông X vaø Y . Thanh tröôït Z duøng ñeå ñieån khieån ñoä cao theo phöông Z . Caùc thanh tröôït V1, V2, V3, V4 laø duøng ñeå ñieàu khieån vaän toác cuûa caùc caùnh A, B, C, D cuûa maùy bay . Phaàn Code cuûa giao dieän ñöôïc ñeå ôû phaàn phuï luïc . VI. LYÙ THUYEÁT VEÀ BOÄ LOÏC KALMAN : Ta ñònh nghóa nkx R∈ laø traïng thaùi tröôùc öôùc löôïng taïi böôùc k, cho bieát traïng thaùi cuûa quaù trình ngay tröôùc traïng thaùi k, vaø nkx R∈ laø traïng thaùi sau öôùc löôïng taïi 1.   Giôùi thieäu boä loïc Kalman: Naêm 1976 R.E. Kalman ñaõ ñöa ra baøi vieát mieâu taû phöông phaùp giaûi hoài quy cho nhöõng baøi toaùn loïc tín hieäu rôøi raïc. Keå töø ñoù, vôùi nhöõng öu ñieåm lôùn trong tính toaùn, boä loïc Kalman laø ñeà taøi cuûa nhieàu nghieân cöùu vaø öùng duïng, cuï theå laø trong lónh vöïc töï ñoäng hay hoã trôï doø ñöôøng. Boä loïc kalman laø taäp hôïp nhöõng phöông trình toaùn hoïc nhaèm tính toaùn caùc giaù trò trung bình ñeå öôùc löôïng traïng thaùi cuûa quaù trình, baèng caùch giaûm thieåu giaù trò trung bình cuûa bình phöông sai soá. Boä loïc kalman raát maïnh ôû nhieàu maët: noù cung caáp giaù trò öôùc löôïng cuûa traïng thaùi tröôùc, hieän taïi, vaø keá tieáp, vaø noù coù theå hoaït ñoäng ngay caû khi khoâng xaùc ñònh ñöôïc traïng thaùi chính xaùc cuûa heä thoáng ñöôïc moâ hình hoaù. Boä loïc kalman ñöôïc phaân laøm 2 loaïi : rôøi raïc (discrete) vaø môû roäng (extended) Trong ñoà aùn naøy nhoùm xin trình baøy nhöõng noäi dung cô baûn veà boä loïc kalman, vaø moät soá ví duï cuï theå. 2.   Boä loïc kalman rôøi raïc ( Discrete Kalman filer) Boä loïc kalman öôùc löôïng quaù trình rôøi raïc coù traïng thaùi x ∈ Rn , vôùi phöông trình update tuyeán tính sau: xk = Axk-1 + Buk-1 + wk-1 Vaø vôùi giaù trò ño * laø: zk = Hxk + vk Hai bieán ngaãu nhieân w vaø v ñaëc tröng cho nhieãu quaù trình vaø nhieãu thieát bò ño. Chuùng ñoäc laäp vôùi nhau, laø nhieãu traéng vaø ñeàu coù xaùc xuaát tuaân theo phaân phoái chuaån: p(w) ~ N(0,Q) p(v) ~ N(0,R) Trong thöïc teá caùc ma traän covariance cuûa nhieãu quaù trình Q vaø nhieãu thieát bò ño R laø thay ñoåi nhöng ôû ñaây ta giaû söû chuùng khoâng thay ñoåi. Ma traän vuoâng nxn A trong phöông trình (1) bieåu dieãn moái quan heä giöõa traïng thaùi tröôùc ñoù vaø traïng thaùi hieän taïi trong tröôøng hôïp khoâng coù nhieãu quaù trình. Chuù yù raèng A thay ñoåi ñoái vôùi töøng traïng thaùi nhöng ôû ñaây ta giaû söû A khoâng ñoåi. Ma traän B nxl bieåu dieãn quan heä giöõa bieán ñieàu khieån ngoõ vaøo vaø traïng thaùi. Ma traän H mxn bieåu dieãn quan heä giöõa giaù trò ño vaø traïng thaùi cuûa quaù trình. Trong thöïc theá, H thay ñoåi nhöng ta giaû söû H khoâng ñoåi. Quaù trình tính toaùn cuûa boä loïc: ^− ^ ek k  − x −K = x Pk−  = E ⎡⎣ek k−T ⎤⎦ e vaø  xk  töø nhöõng giaù trò coù ñöôïc ôû time update, cho neân coøn goïi laø quaù trình chænh söõa böôùc k, vaø cho ra giaù trò ño la zk . Ta ñònh nghóa caùc sai soá tröôùc vaø sau khi öôùc löôïng laø: ^ − ^ ek = xk − xk Ma traän covariance sai soá tröôùc vaø sau öôùc löôïng seõ laø: − k ⎣ k  ⎦ P  = E ⎡ekeT ⎤ Quaù trình xöû lyù cuûa boä loïc kalman chia laøm hai giai ñoaïn : caäp nhaäp thôøi gian ( time update ) vaø caäp nhaäp giaù trò ño ( mesurement update ). Quaù trình update tính toaùn caùc ^ − giaù trò tröôùc öôùc löôïng: ma traän Pk− vaø xk cho neân ñaây coøn goïi laø quaù trình tieân ñoaùn ( predictor). Quaù trình measurement update seõ cho ra nhöõng giaù trò sau khi öôùc löôïng Pk ^ (corrector) . Caùc phöông trình tính toaùn nhö sau: Pk   =  APk−1AT  + Q Time update (“predict”) 1.  Öôùc löôïng giaù trò keá tieáp: xk = Axk-1 + Buk-1 + wk-1 2.  Öôùc löôïng ma traän covariance keá tieáp: −  Measurement update (“ corect”) 1.Tính toaùn ñoä lôïi Kalman: Kk = Pk− H T (HPk− H T + R)−1 2.Caäp nhaäp laïi vôùi giaù trò ño zk ^      ^ −                               ^ − xk = xk + Kk (zk − H xk ) 3.Caäp nhaät laïi ma traän covariance Pk k k−= (I − K H )P Khôûi taïo ^ − xk−1 va Pk-1 Quaù trình tính toaùn cuûa boä loïc DKF Trong ñoù L chính laø heä soá thaëng dö cuûa boä loïc Kalman, hay ñoä lôïi Kalman. Quaù trình tính toaùn cuûa boä loïc kalman laø moät quaù trình qua laïi tuaàn hoaøn giöõa predict vaø corrector . Trong quaù trình thöïc thi thöïc teá cuûa boä loïc, ma traän covariance cuûa nhieãu thieát bò ño R ñöôïc xaùc ñònh tröôùc. R ñöôïc ño off_line baêng caùch cho moät soá giaù trò ño maãu, khaûo saùt vaø xaùc ñònh söï bieán ñoåi cuûa nhieãu ño. Ta cuõng coù theå xaùc ñònh Q töø thöïc nghieäm nhöng khoù khaên hôn nhieàu bôûi vì ñoâi khi ta khoâng theå quan saùt ñöôïc tröïc tieáp quaù trình ñang xaûy ra. Q vaø R chính laø hai thoâng soá ñeå hieäu chænh boä loïc Kalman . Quaù trình hieäu chænh thöôøng ñöôïc thöïc hieän off_line . 3. Moâ hình hoaù gyro: Gyro laø caûm bieán ño vaän toác goùc . Goùc nghieâng theo phöông x cuûa maùy bay ôû traïng thaùi hieän taïi baèng goùc theo phöông x ôû traïng thaùi tröôùc coäng vôùi tích phaân cuûa vaän toác goùc. Ta coù phöông trình sau: èg (k +1) = èg (k) + ù(k).Ät + bg (k) Trong ñoù : èg (k +1) laø giaù trò goùc nghieâng ôû traïng thaùi thöù k+1 èg (k) laø traïng thaùi goùc nghieâng ôû traïng thaùi tröôùc ñoù ù(k) laø vaän toác goùc ño bôûi gyûo ôû traïng thaùi thöù k Ät laø thôøi gian laáy maãu bg (k) laø ñoä troâi cuûa heä soá gyûo bias 3.   Moâ hình hoaù Accelerometer : Töông töï nhö gyro, giaù trò vò trí ta tính ñöôïc cuõng bò sai soá do hai laàn laáy tích phaân. Phöông trình traïng thaùi cuûa accelerometer nhö sau: v(k +1) = v(k) + a(k).Ät + ba (k) xa (k +1) = xa (k ) + v(k).cosè (k ).Ät ya (k +1) = ya (k) + v(k).sinè (k ).Ät Trong  ñoù: v(k ) , v(k +1) laàn löôït laø vaän toác daøi cuûa maùy bay ôû 2 traïng thaùi keá tieáp k vaø k+1 a(k) laø gia toác cuûa xe ôû traïng thaùi thöù k bg(k) laø heä soá troâi cuûa accelerometer xa(k), xa(k) laàn löôït laø vò trí theo phöông x cuûa maùy bay ôû 2 traïng thaùi keá tieáp k vaø k+1 ya(k), ya(k+1) laàn löôït laø vò trí theo phöông y cuûa maùy bay ôû 2 traïng thaùi keá tieáp k vaø k+1. X_DATA Y_DATA Z_DATA NC8 XOUT YOUT ZOUT 5 6 7 8 NC1 NC2 NC3 NC4 16 15 14 13 4.   Thieát keá sô ñoà maïch nguyeân lyù cuûa gyro vaø accelerometer Caûm bieán gia toác maø nhoùm söû duïng laø caûm bieán gia toác 3 truïc , coù nghóa laø vôùi caûm bieán accelerometer naøy coù theå ño ñöôïc gia toác theo 3 phöông X, Y, Z . R1 Z_DATA  1k  C3 ZOUT 0.1uF R2 U1 MMA7260 Y_DATA  1k YOUT C4 0.1uF g-select1 g-select2 3.3V_1 1 2 3 4 g-Select1 g-Select2 VDD VSS ~sleep_mode NC7 NC6 NC5 12 11 10 9  X_DATA R3 1K  XOUT C5 0.1uF Sô ñoà nguyeân lyù caûm bieán gia toác Caûm bieán MMA7260 caàn nguoàn cung caáp laø 3.3 V . Caùc chaân chuùng ta söû duïng ôû ñaây laø chaân g-Select1 ( chaân soá 1) , g-Select2 ( chaân soá 2 ) duøng ñeå choïn cheá ñoä hoaït ñoäng caûm bieán . Ba chaân döõ lieäu  XOUT (15), YOUT (14), ZOUT (13) laø 3 chaân ñöa ra möùc Vol tyû leä vôùi gia toác theo 3 phöông X, Y, Z . Treân moâ hình thí nghieäm nhoùm söû duïng 2 caûm bieán gyro ño theo phöông X vaø Y. U6 5V 3.3V_1 1                    2 5V        NC 3                    4 3.3V       C+  C1 GND DATA DATA1 5 6 C Gy ro Sô ñoà nguyeân lyù caûm bieán gyro Caûm bieán gyro caàn 2 nguoàn cung caáp : 1 nguoàn laø 3.3 V , moät nguoàn laø 5 V . Moät chaân döõ lieäu cho ra möùc Vol tyû leä vôùi vaän toác goùc theo phöông maø gyro ño . 8.Phuï Luïc : Code chöông trình thieát keá giao dieän baèng phaàn meàm MatLab function varargout = GUI1(varargin) % GUI1 M-file for GUI1.fig % GUI1, by itself, creates a new GUI1 or raises the existing % singleton*. % % H = GUI1 returns the handle to a new GUI1 or the handle to % the existing singleton*. % % GUI1('CALLBACK',hObject,eventData,handles,...) calls the local % function named CALLBACK in GUI1.M with the given input arguments. % % GUI1('Property','Value',...) creates a new GUI1 or raises the % existing singleton*.  Starting from the left, property value pairs are % applied to the GUI before GUI1_OpeningFunction gets called.  An % unrecognized property name or invalid value makes property application % stop.  All inputs are passed to GUI1_OpeningFcn via varargin. % % *See GUI Options on GUIDE's Tools menu.  Choose "GUI allows only one % instance to run (singleton)". % % See also: GUIDE, GUIDATA, GUIHANDLES % Copyright 2002-2003 The MathWorks, Inc. % Edit the above text to modify the response to help GUI1 % Last Modified by GUIDE v2.5 28-Feb-2008 13:00:33 % Begin initialization code - DO NOT EDIT gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton',  gui_Singleton, ... 'gui_OpeningFcn', @GUI1_OpeningFcn, ... 'gui_OutputFcn',  @GUI1_OutputFcn, ... 'gui_LayoutFcn',  [] , ... 'gui_Callback',   []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end % End initialization code - DO NOT EDIT % --- Executes just before GUI1 is made visible. function GUI1_OpeningFcn(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFcn. % hObject handle to figure % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % varargin   command line arguments to GUI1 (see VARARGIN) % Choose default command line output for GUI1 handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes GUI1 wait for user response (see UIRESUME) % uiwait(handles.figure1); % --- Outputs from this function are returned to the command line. function varargout = GUI1_OutputFcn(hObject, eventdata, handles) % varargout  cell array for returning output args (see VARARGOUT); % hObject handle to figure % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Get default command line output from handles structure varargout{1} = handles.output; % --- Executes on slider movement. function sliderX_Callback(hObject, eventdata, handles) x=get(handles.sliderX,'value'); x=num2str(x); set(handles.editX,'string',x); set_param('son1/StepX','After',num2str(get(handles.sliderX,'value')/10000)); % hObject handle to sliderX (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider % --- Executes during object creation, after setting all properties. function sliderX_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderX (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderY_Callback(hObject, eventdata, handles) Y=get(handles.sliderY,'value'); Y=num2str(Y); set(handles.editY,'string',Y); set_param('son1/StepY','After',num2str(get(handles.sliderY,'value')/10000)); % hObject handle to sliderY (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider % --- Executes during object creation, after setting all properties. function sliderY_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderY (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderZ_Callback(hObject, eventdata, handles) Z=get(handles.sliderZ,'value'); Z=num2str(Z); set(handles.editZ,'string',Z); % hObject handle to sliderZ (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider % --- Executes during object creation, after setting all properties. function sliderZ_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderZ (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editx_Callback(hObject, eventdata, handles) % hObject handle to editx (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editx as text % str2double(get(hObject,'String')) returns contents of editx as a double % --- Executes during object creation, after setting all properties. function editx_CreateFcn(hObject, eventdata, handles) % hObject handle to editx (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editY_Callback(hObject, eventdata, handles) % hObject handle to editY (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editY as text % str2double(get(hObject,'String')) returns contents of editY as a double y=get(handles.editY,'string'); y=str2num(y); set(handles.sliderY,'value',y); % --- Executes during object creation, after setting all properties. function editY_CreateFcn(hObject, eventdata, handles) % hObject handle to editY (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editZ_Callback(hObject, eventdata, handles) % hObject handle to editZ (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editZ as text % str2double(get(hObject,'String')) returns contents of editZ as a double z=get(handles.editZ,'string'); z=str2num(z); set(handles.sliderZ,'value',z); % --- Executes during object creation, after setting all properties. function editZ_CreateFcn(hObject, eventdata, handles) % hObject handle to editZ (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderV1_Callback(hObject, eventdata, handles) % hObject handle to sliderV1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider V1=get(handles.sliderV1,'value'); V1=num2str(V1); set(handles.editV1,'string',V1); set_param('son1/Pulse Generator1','Amplitude',num2str(1000*get(handles.sliderV1,'value'))); % --- Executes during object creation, after setting all properties. function sliderV1_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderV1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderV2_Callback(hObject, eventdata, handles) % hObject handle to sliderV2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider V2=get(handles.sliderV2,'value'); V2=num2str(V2); set(handles.editV2,'string',V2); set_param('son1/Pulse Generator2','Amplitude',num2str(1000*get(handles.sliderV2,'value'))); % --- Executes during object creation, after setting all properties. function sliderV2_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderV2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderV3_Callback(hObject, eventdata, handles) % hObject handle to sliderV3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider V3=get(handles.sliderV3,'value'); V3=num2str(V3); set(handles.editV3,'string',V3); set_param('son1/Pulse Generator3','Amplitude',num2str(1000*get(handles.sliderV3,'value'))); % --- Executes during object creation, after setting all properties. function sliderV3_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderV3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editv1_Callback(hObject, eventdata, handles) % hObject handle to editv1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editv1 as text % str2double(get(hObject,'String')) returns contents of editv1 as a double % --- Executes during object creation, after setting all properties. function editv1_CreateFcn(hObject, eventdata, handles) % hObject handle to editv1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editv2_Callback(hObject, eventdata, handles) % hObject handle to editv2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editv2 as text % str2double(get(hObject,'String')) returns contents of editv2 as a double % --- Executes during object creation, after setting all properties. function editv2_CreateFcn(hObject, eventdata, handles) % hObject handle to editv2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editv3_Callback(hObject, eventdata, handles) % hObject handle to editv3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editv3 as text % str2double(get(hObject,'String')) returns contents of editv3 as a double % --- Executes during object creation, after setting all properties. function editv3_CreateFcn(hObject, eventdata, handles) % hObject handle to editv3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on slider movement. function sliderV4_Callback(hObject, eventdata, handles) % hObject handle to sliderV4 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider % get(hObject,'Min') and get(hObject,'Max') to determine range of slider V4=get(handles.sliderV4,'value'); V4=num2str(V4); set(handles.editV4,'string',V4); set_param('son1/Pulse Generator4','Amplitude',num2str(1000*get(handles.sliderV4,'value'))); % --- Executes during object creation, after setting all properties. function sliderV4_CreateFcn(hObject, eventdata, handles) % hObject handle to sliderV4 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default.  See ISPC and COMPUTER. usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 .9 .9]); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editV4_Callback(hObject, eventdata, handles) % hObject handle to editV4 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV4 as text % str2double(get(hObject,'String')) returns contents of editV4 as a double V4=get(handles.sliderV4,'value'); V4=num2str(V4); set(handles.editV4,'string',V4); % --- Executes during object creation, after setting all properties. function editV4_CreateFcn(hObject, eventdata, handles) % hObject handle to editV4 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editV1_Callback(hObject, eventdata, handles) % hObject handle to editV1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV1 as text % str2double(get(hObject,'String')) returns contents of editV1 as a double % --- Executes during object creation, after setting all properties. function editV1_CreateFcn(hObject, eventdata, handles) % hObject handle to editV1 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editV3_Callback(hObject, eventdata, handles) % hObject handle to editV3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV3 as text % str2double(get(hObject,'String')) returns contents of editV3 as a double % --- Executes during object creation, after setting all properties. function editV3_CreateFcn(hObject, eventdata, handles) % hObject handle to editV3 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editV2_Callback(hObject, eventdata, handles) % hObject handle to editV2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV2 as text % str2double(get(hObject,'String')) returns contents of editV2 as a double % --- Executes during object creation, after setting all properties. function editV2_CreateFcn(hObject, eventdata, handles) % hObject handle to editV2 (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end function editX_Callback(hObject, eventdata, handles) % hObject handle to editX (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editX as text % str2double(get(hObject,'String')) returns contents of editX as a double x=get(handles.editX,'string'); x=str2num(x); set(handles.sliderX,'value',x); % --- Executes during object creation, after setting all properties. function editX_CreateFcn(hObject, eventdata, handles) % hObject handle to editX (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows. % See ISPC and COMPUTER. if ispc set(hObject,'BackgroundColor','white'); else set(hObject,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor')); end % --- Executes on button press in SimulateButton. function model_open(handles) open_system('son1'); set_param('son1/Pulse Generator1','Amplitude',num2str(1000*get(handles.sliderV1,'value'))); set_param('son1/Pulse Generator2','Amplitude',num2str(1000*get(handles.sliderV2,'value'))); set_param('son1/Pulse Generator3','Amplitude',num2str(1000*get(handles.sliderV3,'value'))); set_param('son1/Pulse Generator4','Amplitude',num2str(1000*get(handles.sliderV4,'value'))); set_param('son1/StepX','After',num2str(get(handles.sliderX,'value')/10000)); set_param('son1/StepY','After',num2str(get(handles.sliderY,'value')/10000)); function SimulateButton_Callback(hObject, eventdata, handles) % hObject handle to SimulateButton (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) model_open(handles); set_param('son1', 'SimulationCommand', 'start') function SimulateButton_CreateFcn(hObject, eventdata, handles) % hObject handle to SimulateButton (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called %them ma set(hObject,'String','Simulate'); % --- Executes on button press in ExitButton. function ExitButton_Callback(hObject, eventdata, handles) % hObject handle to ExitButton (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) close(gui1); % --- Executes during object creation, after setting all properties. function ExitButton_CreateFcn(hObject, eventdata, handles) % hObject handle to ExitButton (see GCBO) % eventdata  reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % them ma set(hObject,'String','Exit'); Code cuûa haøm tính toaùn goùc nhieâng döïa vaøo thuaät toaùn  Kalman extern float angle; extern float q_bias; extern float rate; extern void state_update( ); const float      q_m   /* Pitch gyro measurement */ extern void kalman_update( const float ax_m,   /* X acceleration */ const float az_m   /* Z acceleration */ ); #endif Haøm  kalman_update #include  /* * thôøi gian laáy maãu */ static const float   dt   = ( 1024.0 * 256.0 ) / 8000000.0; /* * Ma traän covariance , ma traän naøy ñöôïc caäp nhaäp sau moãi chu kyø tính * Ma traän naøy laø ma traän covarian sai soá tröôùc khi tính */ static float P[2][2] = { { 1, 0 }, { 0, 1 }, }; /* * goùc nhieâng * bias * toác ñoä */ float angle; float q_bias; float rate; /* * R laø ma traän covarian cuûa nhieuã ño . Trong tröôøng hôïp naøy * laø ma traän 1x1 , chuùng ta ñaët laø 0.3 rad ( thoâng soá naøy ño ñöôïc töø thöïc nghieäm) */ static const float   R_angle   = 0.3; /* * Q laø ma traän 2x2 trình baøy nhieãu quaù trình */ static const float   Q_angle   = 0.001; static const float   Q_gyro   = 0.003; /* *   X = [ angle, gyro_bias ] *   Xdot = [ angle_dot, gyro_bias_dot ] *   angle_dot   = gyro - gyro_bias *   gyro_bias_dot   = 0 *   Pdot = A*P + P*A' + Q * * A laø Jacobian cuûa Xdot ñöôïc tính nhu sau: * *   A = [ d(angle_dot)/d(angle) d(angle_dot)/d(gyro_bias) ] * [ d(gyro_bias_dot)/d(angle) d(gyro_bias_dot)/d(gyro_bias) ] * * = [ 0 -1 ] * [0  0] */ void state_update( const float q_m   /* Pitch gyro measurement */ ) {  const float      q = q_m - q_bias; /* *   Pdot = A*P + P*A' + Q * *  A = [ 0 -1, 0 0 ] *  A' = [ 0 0, -1, 0 ] *  Q_angle vaø Q_gyro coù trong ma traän Q */ const float Pdot[2 * 2] = { Q_angle - P[0][1] - P[1][0],   /* 0,0 */ - P[1][1], /* 0,1 */ - P[1][1], /* 1,0 */ Q_gyro /* 1,1 */ }; rate = q; /* * goùc sau khi öôùc löôïng * angle += angle_dot * dt * += (gyro - gyro_bias) * dt * += q * dt */ angle += q * dt; /* Caäp nhaäp ma traän P */ P[0][0] += Pdot[0] * dt; P[0][1] += Pdot[1] * dt; P[1][0] += Pdot[2] * dt; P[1][1] += Pdot[3] * dt; } /* *   C = [ d(angle_m)/d(angle)  d(angle_m)/d(gyro_bias) ] * =[10] */ void kalman_update( const float ax_m,   /* X acceleration */ const float az_m   /* Z acceleration */ ) { /* tính goùc ño vaø sai soá öôùc löôïng */ const float angle_m = atan2( -az_m, ax_m ); const float angle_err = angle_m - angle; /* * C_0 trình baøy söï lieân quan giöõa giaù trò ño vaø traïng thaùi öôùc löôïng * * The C_1 trình baøy söï lieân quan giöõa traïng thaùi ño vôùi heä soá bias cuûa gyro */ const float C_0 = 1; /* const float C_1 = 0; */ /* * PCt = P * C' */ const float PCt_0 = C_0 * P[0][0]; /* + C_1 * P[0][1] = 0 */ const float PCt_1 = C_0 * P[1][0]; /* + C_1 * P[1][1] = 0 */ /* *   E = C P C' + R *   E = C P C' + R */ const float E = R_angle + C_0 * PCt_0 /*   + C_1 * PCt_1 = 0 */ ; /* *   K = P C' inv(E) *   K = P C' inv(E) *  E laø , vì theá inverse cuûa E chæ laø 1/E. */ const float K_0 = PCt_0 / E; const float K_1 = PCt_1 / E; /* *   P=P-KCP *   P -= K C P *   t[0,0] = C[0,0] * P[0,0] + C[0,1] * P[1,0] *   t[0,0] = C[0,0] * P[0,0] = PCt[0,0] */ const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] */ const float t_1 = C_0 * P[0][1]; /* + C_1 * P[1][1]  = 0 */ P[0][0] -= K_0 * t_0; P[0][1] -= K_0 * t_1; P[1][0] -= K_1 * t_0; P[1][1] -= K_1 * t_1; /* *   X += K * err *   X = X + K * err */ angle   += K_0 * angle_err; } q_bias   += K_1 * angle_err;