May Bay 4 Canh

8/8/2019 May Bay 4 Canh

http://slidepdf.com/reader/full/may-bay-4-canh 1/41

http://www.ebook.edu.vn

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Ể NQUADROCOPTER

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 VisualNastran5. Xây dựng mô hình điều khiển QuadroCopter trong Simulink của MatLab6. 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ôøcoù4 caùnh neân loaïi maùy bay naøy deãñieàu khieån hônvaøcoùkhaûnaêng giöõthaêng baèng toát hôn . Loaïi maùy bay naøy coùraát nhieàu öùng duïng thöïcteá, noùcoùtheåcoùkích côõlôùn ñeåchôûngöôøi vaøhaøng hoaùvaøcuõng coùtheåcoùkích côõnhoûraát naêng ñoäng coùtheåhoaït ñoäng ngay caûtrong nhaø. Loaïi maùy bay kích côõbay trongnhaøhieän nay ñang phaùt trieån raát raàm roä, nhieàu sinh vieân cuûa Ñöùc vaøMyõñaõcheátaïoloaïi maùy bay naøy vôùi nhieàu hình daùng vaøtính naêng khaùc nhau ñeånghieân cöùu . Trongphaàn ñoàaùn naøy nhoùm xin ñöôïc giôùi thieäu keát quaûnghieân cöùu veàloaïi Quadrocopter

hoaït ñoäng trong nhaø.Trong ñoàaùn naøy nhoùm ñaõmoâphoûng maùy bay Quadrocopter baèng phaàn meàm

MatLab keát hôïp vôùi Visual Nastran . Sau ñoùnhoùm ñaõthieát keáphaàn cöùng maùy bay vaøhoaøn thaønh moät soámodul nhoûcuûa coâng vieäc ñieàu khieån maùy bay . Trong ñoàaùn naøy coùñöa vaøo modul ño goùc nhieâng cuûa maùy bay baèng caûm bieán gyro vaøcaûm bieán gia toácdaøi ñöôïc öôùc löôïng baèng boäloï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ì quaycù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ạchtruyền sóng để k ết nối vớ i bộ điều khiển ở dướ i đất , mạch chính dùng để điềukhiể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ếnnhư cảm biến gyro để đo độ nghiêng, cảm biển aùp suaát để đo độ cao , cảm biểngia 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óngvô 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ôngthườ 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áy2 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ønlaï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ïcvuoâng goùc vôùi maët phaúng maùy bay, theo nguyeân lyùbaûo toaøn momanh ñoäng löôïngthì 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ättieâ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ûmtoá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ùnhquaï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öôngngang .

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ïngnguyeân taéc ñieàu khieån treân vaøo phöông ñoù.

Xoay voøng quanh tr uï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ønlaïi ñeågiaûm momanh ñoäng löôïng theo phöông ngöôïc vôùi chieàu xoay mong muoán cuûamaù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àmVisual Nastran keát hôïp vôùi Simulink cuûa Matlab giuùp ta coùmoät moâi tröôøng moâphoûnggaà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âydöï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ûanoù. Vieäc tính toaùn haøm truyeàn naøy ñöôïc thöïc hieän trong moâi tröôøng MatLab , sau khitính ñöôïc löïc naâng , ta ñöa löïc naøy vaøo trong moâi tröông Visual Nastran ( do trongNastran 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 4caù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àukhieå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øycuõ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ùolöï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 TRONGSIMULINK 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ểukhiể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 ).




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ủacá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ônggó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ảithay đổ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ốcgố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ớ iVisualNastran .

Dướ i đây là mô hình điều khiển độ nghiêng của máy bay :

X

Y




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 control ler2 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 : K p = 30 , K I = 15 , K D = 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 trongkhoả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 ñöôïcmöù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 khiqua 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 đưara 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änvôù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ìnhGiao 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àukhieå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 :




1. Giôùi thieäu boäloïc K alman:

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 chonhöõ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òtrungbì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 r aïc ( Discrete Kalman filer )

Boäloïc kalman öôùc löôïng quaùtrình rôøi raïc coùtraïng thaùi n x R∈ , 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áichuaå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 Bnxl 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 mxnbieå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:

Ta ñònh nghóa^

n

k x 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ø^

n

k x R∈ laøtraïng thaùi sau öôùc löôïng taïi




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 covariancekeátieáp:

1T

k k P AP A Q

−−= +

Measurement update (“ corect”)1.Tính toaùn ñoälôïi Kalman:

1( )T T

k k k K P H HP H R− − −= +

2.Caäp nhaäp laïi vôùi giaùtròño zk ^ ^ ^

( )k k k k k x x K z H x− −

= + −

3.Caäp nhaät laïi ma traän covariance( )

k k k P I K H P−= −

Khôûi taïo^

1k x−

− va Pk-1

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öôïnglaø:

^

^

K k k

k k k

e x x

e x x

−− = −

= −

Ma traän covariance sai soátröôùc vaøsau öôùc löôïng seõlaø:

T

k k k

T

k k k

P E e e

P E e e

− − −⎡ ⎤= ⎣ ⎦

⎡ ⎤= ⎣ ⎦

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 ( timeupdate ) 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änk

P− vaø

^k x−

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

vaø^

k x töønhöõng giaùtròcoùñöôïc ôûtime update, cho neân coøn goïi laøquaùtrình chænh söõa(corrector) . Caùc phöông trình tính toaùn nhö sau:

Quaùtrình tính toaùn cuûa boäloïc DKFTrong ñ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öõapredict 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átbòño R ñöôïc xaùc ñònh tröôùc. R ñöôïc ño off_l ine 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öïcnghieä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ápquaù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äuchænh thöôøng ñöôïc thöïc hieän of f_l ine .

3. Moâhình hoaùgyr o:

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ìnhsau:

( 1) ( ) ( ). ( )g g g

k k k t b k θ θ ω + = + Δ +

Trong ñoù:( 1)

gk θ + 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( )gb 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:

( 1) ( ) ( ). ( )

( 1) ( ) ( ).cos ( ).

( 1) ( ) ( ).sin ( ).

a

a a

a a

v k v k a k t b k

x k x k v k k t

y k y k v k k t

θ

θ

+ = + Δ +

+ = + Δ

+ = + Δ

Trong ñoù:

( )v k , ( 1)v k + 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öùkbg(k) laøheäsoátroâi cuûa accelerometerxa(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+1ya(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.




4. Thieát keásô ñoàmaïch nguyeân lyùcuûa gyr o vaøacceler ometer

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ûmbieán accelerometer naøy coùtheåño ñöôïc gia toác theo 3 phöông X, Y, Z .

g-select2

X_

D A T A

3.3V_1

U1

MMA7260

1234

5 6 7 8

9101112

1 3

1 4

1 5

1 6

g-Select1g-Select2VDDVSS

N C 1

N C 2

N C 3

N C 4

NC 5NC 6NC 7

~sleep_mode

Z O U T

Y O U T

X O U T

N C 8

Z_

D A T A

g-select1

Y_

D A T A

ZOUT

R3

1K

YOUT

Z_DATA

C4

0.1uF

XOUT

Y_DATA

R2

1k

C3

0.1uF

C5

0.1uF

R1

1k

X_DATA

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âylaøchaân g-Select1 ( chaân soá1) , g-Select2 ( chaân soá2 ) duøng ñeåchoïn cheáñoähoaït ñoängcaûm bieán . Ba chaân döõlieäu XOUT (15), YOUT (14), ZOUT (13) laø3 chaân ñöa ra möùcVol 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.

5V

3.3V_1

U6

Gyro

1 2

3 4

5 6

5V NC

3.3V C+

GN D D ATADATA1

C1

C

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ândöõ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-f ile 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 EDITgui_Singleton = 1;gui_State = struct('gui_Name', mfi lename, ...

'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{ :} );

elsegui_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 wi th handles and user data (see GUIDATA)% varargin command line arguments to GUI1 (see VARARGIN)

% Choose default command line output for GUI1handles.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 wi th handles and user data (see GUIDATA)

% Get default command line output f rom handles structurevarargout{ 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 wi th 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]);

elseset(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 wi th handles and user data (see GUIDATA)





% --- 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


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 wi th handles and user data (see GUIDATA)


% --- 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







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 wi th 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 ispcset(hObject,'BackgroundColor','white');


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 wi th 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 doubley=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


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 wi th 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







% --- 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 wi th 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 sliderV1=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



% --- 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 wi th 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 sliderV2=get(handles.sliderV2,'value');V2=num2str(V2);set(handles.editV2,'string',V2);




set_param('son1/PulseGenerator2','Amplitude',num2str(1000*get(handles.sliderV2,'value')));



usewhitebg = 1;if usewhitebg



% --- 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 wi th 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 sliderV3=get(handles.sliderV3,'value');V3=num2str(V3);set(handles.editV3,'string',V3);set_param('son1/PulseGenerator3','Amplitude',num2str(1000*get(handles.sliderV3,'value')));






usewhitebg = 1;if usewhitebgset(hObject,'BackgroundColor',[.9 .9 .9]);


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 wi th 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




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 wi th handles and user data (see GUIDATA)





% --- 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


if ispcset(hObject,'BackgroundColor','white');


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 wi th handles and user data (see GUIDATA)


% --- 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



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 wi th 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 sliderV4=get(handles.sliderV4,'value');

V4=num2str(V4);set(handles.editV4,'string',V4);set_param('son1/PulseGenerator4','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




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 wi th 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






% --- 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





if ispcset(hObject,'BackgroundColor','white');else




% --- 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






% --- 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




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 wi th 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 doublex=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




% --- 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')));




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 voidstate_update(

const float q_m /* Pitch gyro measurement * /);

extern voidkalman_update(

const float ax_m, /* X acceleration * /const float az_m /* Z acceleration * /

);

#endif

Haøm kalman_update

#include <math.h>

/** 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) ]

* = [ 1 0 ]* /voidkalman_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<2,1> = P<2,2> * C'<2,1>* /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<1,1> = C<1,2> P<2,2> C'<2,1> + R<1,1>* /const float E =

R_angle+ C_0 * PCt_0

/* + C_1 * PCt_1 = 0 * /;

/** K = P C' inv(E)* K<2,1> = P<2,2> C'<2,1> inv(E)<1,1>



* E laø<1,1>, 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 - K C P* P<2,2> -= K<2,1> C<1,2> P<2,2>* 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<2> = X<2> + K<2,1> * err<1,1>

* /angle += K_0 * angle_err;q_bias += K_1 * angle_err;

}

May Bay 4 Canh

Documents

Transcript of May Bay 4 Canh