Luận án Tổng hợp hệ điều khiển tay máy có khớp đàn hồi ứng dụ

 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ 
TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI 
ỨNG DỤ 
- NĂM 2016
 BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ QUỐC PHÒNG 
VIỆN KHOA HỌC VÀ CÔNG NGHỆ QUÂN SỰ 
TỔNG HỢP HỆ ĐIỀU KHIỂN TAY MÁY CÓ KHỚP ĐÀN HỒI 
ỨNG DỤ 
Chuyên ngành: khiển và T 
Mã số: 62.52.02.16 
 NGƢỜI HƢỚNG DẪN KHOA HỌC: 
1. PGS.TS TRẦN ĐỨC THUẬN 
- NĂM 2016
i 
Tôi cam đoan đâ tôi
ai trên 
01 8 năm 2016 
ii 
ần Đức Thuậ
, ể
K
q . 
m ơn c
C –
. 
iii 
 ................................................................... vi 
 ................................................................................. ix 
ầu ........................................................................................................................ 1 
Chƣơng 1: TỔNG QUAN VỀ
 ................................................................... 5 
1.1. ệp ........................................................................ 5 
1.1.1. Sơ lượ ệp ............... 5 
1.1.2 y ................................................................. 6 
1.1.3 ................................................................. 10 
1.2. . .. 11 
1.3. Tình hình nghiên cứu ......................................................... 14 
1.4. ều khiển ch . .. 17 
 ..................................................................................................... 20 
Chƣơng 2: XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TƢ THẾ GÓC VÀ CÁC 
THUẬT TOÁN HIỆU CHỈNH KHI SỬ DỤNG CÁC 
VI CƠ QUÁN TÍNH ................................................................... 21 
2.1. Giới thiệu thuật toán lọc Kalman ........................................................................ 21 
2.2. Xây dựng thuật toán xác đị ứng dụng các phần tử
ố ......... 22 
2.2.1. Ma trận cosin chỉ phương và các góc định hướng .................................... 22 
2.2.2. Xây dựng thuật toán xác định tham s ......................... 27 
2.3. Xây dựng thuật toán xác định tham số góc cho tay máy ứng dụng các phần tử 
vi cơ trong trường hợ ố .................................. 32 
2.4. Xây dựng thuật toán hiệu các thông số đo ................................................ 34 
2.4 ... 34 
2.4.2. Phương pháp và thuật toán hiệu chuẩn ..................................................... 37 
2.5. Xây dựng thuật toán nhận dạng hiệu ch hệ đo ............................................... 46 
2.5.1. Phương pháp hiệu chỉnh 6 vị trí. ............................................................... 47 
2.5.2. Phương pháp hiệu chỉnh cho hệ đo véc tơ tốc độ góc. ............................. 49 
2.5.3. Phương pháp hiệu chỉnh cho hệ đo véc tơ gia tốc. ................................... 52 
Kết luận chƣơng 2 ..................................................................................................... 54 
iv 
Chƣơng 3: XÂY DỰNG THUẬT TOÁN ĐIỀU KHIỂN CHO TAY MÁY
 .............................................................................................. 55 
3.1. Cơ ............................................................................... 55 
3.1.1. Tính ổn định Lyapunov ............................................................................. 55 
3.1.2. Hàm điều khiển Lyapunov ........................................................................ 57 
3.2. Hệ thống phản hồ ........................................................................................ 58 
 .................................................... 61 
3.4. Tổng hợp bộ điều khiển cho tay rô bốt .................................. 65 
3.4.1. Mô hình đối tượng .................................................................................... 65 
3.4.2. Thiết kế bộ điều khiển ............................................................................... 66 
Kết luận chƣơng 3 ..................................................................................................... 80 
Chƣơng 4: ỀU 
KHIỂN 
 .............................................................................................. 81 
4.1. Mô ậ
 .......................................................................... 81 
4.1.1. Mô phỏng kiểm nghiệm thuậ ... 81 
4.1.2. .................................... 83 
4.1.3. .................................... 85 
4.2. ...................................................... 87 
4.2.1. ......................................... 87 
4.2.2. 
 ............................................................................................. 90 
Kết luận chƣơng 4 .................................................................................................... 98 
 ............................................................................................... 100 
 ..................................................... 101 
 .................................................................................................... 103 
 ....................................................................................................................... P1 
v 
DANH MỤC CÁC CÁC KÝ HIỆ CHỮ VIẾT TẮT 
1. Ký hiệu Ý nghĩa 
A Ma trận 
ˆ ˆ ˆ, ,x y zA A A x, y, z 
, ,c c c
x y z
A A A x, y, z 
ax, ay, az 
2
] 
ˆ ˆ ˆ, ,
x y z
B B B x, y, z 
, ,c c c
x y z
B B B 
b1, b2, b3 Các tham số biến đổi chậm, thể hiện độ trôi của con quay 
F
1
, F
2
 Các hằng số ma sát nhớt 
1kF Véctơ hàm số F ở bước thứ k-1 
G 
Gx, Gy, Gz Các gia tốc kế x, y, z 
Hk hi(Xk xj k 
I 
i Dòng cảm ứng [A] 
J
1
, J
2
2
] 
K Hệ số lò xo 
Kb Hệ số phản hồi suất điện động 
Kt Hằng số mô men 
m 
1 2,c cM M Mô men ma sát cơ 
L Cảm kháng phần ứng (Ω) 
N 
Pk 
( )kP Pk. 
( )kP Pk. 
Q Ma trận đối xứng 
vi 
Qk 
q
1
, q
2
R Điện trở phần ứng (Ω) 
Rk 
Tx, Ty, Tz 
u Điện áp phần ứng [V] 
V Hàm Lyapunov 
Vi Hàm Lyapunov thứ i 
X Véc tơ trạng thái của hệ thống 
kX Trạng thái của véctơ trạng thái X ở bước thứ k 
1kX Trạng thái của véctơ trạng thái X ở bước thứ k-1 
ˆ ( )kX 
X k 
ˆ ( )kX 
X k 
xi rạng thái thứ i của hệ thống điều khiển 
Y Véc tơ đầu ra của hệ thống 
Z 
kZ Giá trị của véctơ k 
ˆ
kZ 
Z k 
zi Sai số hiệu chỉnh 
kk , Véctơ nhiễu động lực và nhiễu đo có dạng ồn trắng 
1k fi(Xk-1 xj 
k-1 
 Kỳ vọng toán học 
, , ớng , góc chúc ngóc và góc Cren) 
, ,x y z Tốc độ quay thực của các con quay 
o
x
, oy ,
o
z Chỉ 
, ,x y zw w w Các nhiễu đo thường có dạng ồn trắng 
1 2 8, ,..., Các tham số không xác định 
vii 
i 
Sai số đánh giá 
, ,ic p 
Các hằng số dương 
i 
Điều khiển ảo 
i 
Sai số bộ quan sát 
2. Chữ viết tắt Ý Nghĩa 
AMF Công ty Đúc Máy American Machine and 
Foundry Company) 
BĐK Bộ điều khiển 
CLF n (Control Lyapunove Function) 
CNC Máy gia công CNC (Computer Numerical Control) 
DH Denavit-Hartenberg 
ĐK Điều khiển 
IMU Inertial Measurement Unit) 
KĐCS Khuếch đại công suất 
LF Hàm Lyapunove (Lyapunove Function) 
MEMs Micro Electro Mechanical Systems) 
OSC Máy hiện sóng (Oscilloscope) 
P iểu khiển tỷ lệ (Proportional) 
PC Máy tính cá nhân (Personal Computer) 
PD Bộ điểu khiển tỷ lệ, đạo hàm (Proportional Derivative) 
PI Bộ điểu khiển tỷ lệ, tích phân (Proportional Integral) 
PID Bộ điểu khiển tỷ lệ, tích phân và đạo hàm (Proportional Integral 
Derivative) 
PWM Bộ điều chế độ rộng xung (Pulse Width Modulation) 
viii 
DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ 
 Trang 
 6 
 7 
Hình 1.3. Chiều dài và góc xoắn của một khâu 8 
Hình 1.4 : Các thông số ai, αi, di và θi 8 
 9 
rô 10 
 12 
 12 
 14 
 18 
 18 
Hình 1.12. Độ lệch giữa hệ quy chuẩn đo và hệ quy chuẩ 19 
ệ tọa độ liên kết và hệ tọa độ chuẩn 23 
ệ toạ độ liên kết và hệ toạ độ 
chuẩn 
24 
 25 
ồ bố trí con quay vi cơ đo tốc độ góc, gia tốc kế và từ kế 27 
Hình 2.5. 33 
Hình 2.6 ệ tọa độ liên kế 35 
Hình 2.7. Hệ tọa độ liên kết và các gia tốc kế 38 
Hình 2.8. Hệ tọa độ cơ sở và hệ toạ độ liên kết 46 
 vị trí cần có để xác định các hệ số hiệu chuẩn 48 
 55 
 59 
ix 
 63 
 65 
11 và X1 82 
21 và X4 83 
Hình 4.3. Đồ thị tọa độ véc tơ từ trường trong hệ tọa độ liên kết 85 
Hình 4.4. Tham số thực và tham số nhận dạng đối với hệ đo tốc độ góc 86 
 87 
 88 
a. Kết quả thử nghiệm khi quay đối tượng trong không gian 89 
b. Kết quả sai lệch khi quay vật thể trong mặt phẳng ngang 89 
IMU trên bàn xoay ba bậc tự do 90 
 90 
Hình 4.10 Sơ đồ cấu trúc của thiết bị thí nghiệm 91 
 92 
 93 
 94 
Hình 4.14. Tín hiệu góc đặt, góc thực tế q1 (a) và tín hiệu vận tốc góc ω2 (b) 95 
Hình 4.15. Tín hiệu điện áp u m =5 [kg] 96 
Hình 4.16. Tín hiệu điện áp u điều khiển PID m =5 [kg], KP=4, KI=1, 
KD=2 
96 
 m1 =5kg; K1=10[Nm/rad], KP=4, KI=1, KD=2 
96 
 m2 =10 [kg]; K1=10[Nm/rad], KP=4, KI=1, KD=2 
97 
Hình 
 m2 =10[kg], K2=20[Nm/rad], KP=1, KI=1, KD=1 
97 
1 
MỞ ĐẦU 
Trong quá trình công nghiệp hoá, hiện đại hoá đất nước, các ngành công 
nghiệp đang phát triển hết sức nhanh chóng, nhiều nhà máy xí nghiệp được xây 
dựng với quy mô và công nghệ hiện đại, tiên tiến đáp ứng được nhu cầu của tình 
hình sản xuất hiện nay, trong đó phải kể đến sự tiến bộ vượt bậc của khoa học kỹ 
thuật, nhất là sự ra đời của máy tính và công nghệ thông tin đã tạo tiền đề cho sự 
phát triển mạnh mẽ của nền sản xuất có tính chất tự động hoá cao, đã dần thay thế 
sức lao động của con người đồng thời hiệu quả của nó đem lại cho nền kinh tế là rất 
lớn. 
Hệ thống nhận dạng và điều khiển đối tượng truyền động có yếu tố phi tuyế
ngườ ự xuất hiện của công nghệ đo lường vi cơ quán tính 
và phi quán tính đã được ứng dụ ực điều khiển khác nhau như điều 
khiển vật thể chuyển động hay trong các máy công cụ , 
vấn đề áp dụng cho tay máy và người máy vẫ ớ
giải quyết. ứ ựng hệ 
điều khiển tượng truyền động có yếu tố phi tuyến 
trên “Tổng hợp hệ điều khiển tay máy có khớp đàn 
hồi ứng dụ “. 
 : 
Kết quả nghiên cứu của luận án là cơ sở khoa họ
ựng phần mềm cho hệ thống điều khiể
ể cải tiến, hiện đạ
. 
2 
truyền 
động có yếu tố phi tuyến nh ệc ứng dụng các 
thiết bị đo quán tính vi cơ điện tử cho vấn đề xác đ nh tham số định hướng của tay 
máy trong xây dựng thuậ
ệ
chuẩ , 
 : 
 cơ v 
hệ thống nhận dạng và điều khiển 
 : 
- 
việc ứng dụng các thiết bị đo quán tính vi cơ điện tử xác đ nh tham số định 
hướ ệ ẩ 
- ên 
Matlab/Simulink, ... 
ốt có khớp nối mềm. 
3 
Chƣơng 1: Tổng quan về
 việc ứng dụng các thiết bị đo quán tính vi cơ điện tử cho vấn đề xác đ nh tham số 
định hướng của tay máy. 
 2, 
Chƣơng 2: Xây dựng thuật toán xác định tƣ thế góc và các thuật toán 
, hiệu chỉnh khi sử dụng các vi cơ quán tính 
ệu chuẩn và hiệu chỉnh hệ đo qụán 
t ật toán quan trọng khi ứng dụng các 
 cũng như phi quán tính sử dụng trong thực tế. Thuật toán hiệu chuẩn dùng 
để khắc phục các sai số không thể tránh khi chế tạo các thiết bị đo quán tính và 
phi quán tính. Thuật toán hiệu chỉnh dùng để khắc phục các sai số p đặt thiết bị 
đo quán tính cũng như phi quán tính vào vật thể chuyển động. Việc xây dự ật 
toán này là cần thiết trong giai đoạn hiệ ều ứng dụng các phần tử
, cơ cấu dịch chuyển cơ khí... 
2, 4, 5 
Chƣơng 3. Xây dựng thuật toán điều khiển khớp 
y thủ tục thiết kế bộ điều khiển 
nghi cho . Luận án đi sâu vào nghiên cứu xây dựng bộ điều 
khiển cho tham số thay đổi, chỉ có đầu ra 
được đo và là tín hiệu phản hồi duy nhất trong vòng điều khiển, ở đó phần phi tuyến 
chỉ phụ thuộc vào các tín hiệu đầu ra.Với hệ thống này, luận án xây dựng bộ quan sát 
phi tuyến dạng hàm mũ hội tụ và thay thế trạng thái không đo được bởi giá trị ước 
lượng của chúng. Thủ tục thiết kế bộ điều khiển dựa theo định lý Lyapunov 
sẽ thực hiện song hành cùng việc tính toán bộ quan sát để ước lượng các trạng thái 
không đo lường được của hệ. Các tham số thay đổi của hệ cũng được tính toán, ước 
lượng bởi các luật cập nhật, đảm bảo tính thích nghi tham số cho hệ có tham số thay 
đổi. 
4 
 3 10
Chƣơng 4: M điều khiể
C của luậ trình bày xác đị
,
,
ều khiể 
 3, 9
5 
Chƣơng 1: 
TỔNG QUAN VỀ 
1.1. công nghiệp 
1.1.1. Sơ lượ công nghiệp 
Thuật ngữ “ ” xuất phát từ tiếng Sec (Czech) “Robota” có nghĩa là công 
việc tạp dịch trong vở kịch “Rossum’s Universal Robots của Karel Capek” vào năm 
1921. Trong vở kịch này, Rossum và con trai của ông ta đã chế tạo ra những chiếc 
máy gần giống với con người để phục vụ con người [4]. Có lẽ đó là một gợi ý ban đầu 
cho các nhà sáng chế kỹ thuật về những cơ cấu, máy móc bắt chước các hoạt động cơ 
bắp của con người. 
Đầu thập kỷ 60, công ty Mỹ AMF (American Machine and Foundry Company) 
quảng cáo một loại máy tự động vạn năng và gọi là “Người máy công nghiệp” 
(Industrial Robot). Ngày nay người ta đặt tên người máy công nghiệp (hay công 
nghiệp) cho những loại thiết bị có dáng dấp và một vài chức năng như tay người được 
điều khiển tự động để thực hiện một số thao tác sản xuất. 
Về mặt kỹ thuật, những công nghiệp ngày nay, có nguồn gốc từ hai 
lĩnh vực kỹ thuật ra đời sớm hơn đó là các cơ cấu điều khiển từ xa và các máy công cụ 
điều khiển số. Các cơ cấu điều khiển từ xa thay thế cho cánh tay của người thao tác
nhìn chung việc sử dụ ở các nước công nghiệp phát triển như: Nhật, Mỹ, Đức, 
Italy, Pháp, Anh là rất phổ biến và tốc độ phát triển hằng năm rất nhanh [6]. Trong mỗi 
lĩnh vực nêu trên sự phân bố đối với mỗi loại hình công việc cũng rất khác nhau. Ví dụ 
chỉ riêng trong lĩnh vực đóng gói và lắp ráp người ta thấy ở các nước như Nhật Bản, 
Đức, Anh cứ 1000 công nhân lắp ráp có khoảng hơn 250 chiếm khoảng 25%. 
Trong lĩnh vực lắp ráp điện tử chiếm tới khoảng 40%. Nếu tính chung trong lĩnh vực 
lắp ráp cơ khí hiện nay r đã chiếm tới gần 50% [9]. 
Ngày nay, r được dùng hầu hết trong các ngành công nghiệp, đặc biệt trong 
những ngành có môi trường làm việc độc hại 
như: lắ
. 
6 
(a) (b) 
nh 1.1. : P 
Để hoàn thành những nhiệm vụ trên thì rô b t cần có khả năng cảm nhận các 
thông số trạng thái của môi trường và tiến hành các hoạt động tương tự con người: 
- Khả năng hoạt động của rô b t được đảm bảo bởi hệ thống cơ khí gồm cơ 
cấu vận động để đi lại và cơ cấu hành động để có thể làm việc. Việc thiết kế và chế 
tạo hệ thống này thuộc lĩnh vực khoa học về cơ cấu truyền động, chấp hành và vật 
liệu cơ khí. 
- Chức năng cảm nhận của r gồm thu nhận tín hiệu về trạng thái môi trường và 
trạng thái của bản thân hệ thống, do các cảm biến (sensor) và các thiết bị khác đảm 
nhiệm. Hệ thống này gọi là hệ thống thu nhận và xử lý số liệu hay hệ thống cảm biến
. 
- Muốn phối hợp hoạt động của hai hệ thống trên và r hoạt động theo 
đúng chức năng mong muốn của con người thì phải có hệ thống điều khiển. 
 Như vậy, 
một ngành khoa học chuyên nghiên cứu, 
thiết kế chế tạo các , ứng dụng chúng các lĩnh vực hoạt động khác nhau 
của xã hội, như nghiên cứu khoa học - kỹ thuật, kinh tế, quốc phòng và dân sinh. 
1.1.2. Mô hình động học 
1.1.2.1. Ma trận quan hệ 
b t được coi là một tay máy có một vài bậc tự do có 
thể coi là một tập hợp các khâu gắn liền với các khớp [13]. 
7 
. 
O X0
Z0
Y0
Y1
Z1
O1
I
III
II
X1
X2Z2
Y2
O2
ỗi khâu  ... nce Publication John Wiley & Sons, Inc. 
[28] . La Sall J. and Lefchetz S. (1961), “Stability by Liapunov’s direct method with 
applications”, Academic Press, New York. 
[29] . M. S. Grewal and A. P. Andrews (2001), “Kalman Filtering: Theory and Practice 
Using MATLAB”, Wiley Interscience. 
[30] . Ozgur Yeniay (2005), “A comparative study on optimization method for the 
constrained nonlinear programming problems, mathematical problems 
inEngineering”. 
[31] . P.M Taylor (1990), “Robotic control”, printed in the London. 
[32] . P.T Boggs and J.W Tolle (2000), “Sequential quadratic programming for 
 Largge - scale nonlinear optimization”, J. Comput. Appl. Math. 
[33] . Parviz E. Nikravesh (1988), “Computer -Aided Analysis of Mechnical Systems” 
 printed in USA, pp.19-250. 
[34] . Paul R.P., Robot Manipulators: Mathematics, Programming and Control. 
[35] . Salim Ibrir (2009), “Algebraic observer design for a class of Uniformly-
observable nonlinear systems, Application to 2-link Robotic manipulator”, 
Proceedings of the 7th Asian Control Conference, Hong Kong, China. 
[36] . Salychev O.S (1998) “Inertial Systems in Navigation and Geophysics”, Bauman 
MSTU Press, Moscow. 
[37] . T.Wang (2001), “Global optimization for Constrained nonlinear programming” 
Department of Computer Science , University of Illinois , Illinois, pp. 1-40. 
[38] . W. Khalil, E.Dombre (2004), “Modeling, Identification and Control of Robots”, 
Kogan page Science, London. 
[39] . Wei Sun, Yaonan Wang (2004), “A Recurrent Fuzzy Neural Network Based 
Adaptive Control and Its Application on Robotic Tracking Control”, Neural 
[40] . Withit Chatlatanagulchai, Peter H. Meckl (2005), “Backstepping High-Order 
Differential Neural Network Control of Flexible-Joint Manipulator, American 
Control Conference. 
106 
Tiếng Nga: 
[41] . В. В. Матвеев, В. Я. Распопов. «Основы Построения Бесплатформенных 
Инерциальных Навигационных Систем» - СПб: ГНЦ РФ ОАО «Концерн 
«ЦНИИ «Электроприбор», 2009. 
[42] . Распопов В.Я. Микросистемная Авионика, учебное Пособие.Тула "Гриф и 
к" 2010. 
[43] . Фильтрация и Стохастическое Управление в Динамических Системах. Под 
Редакциѐй К Т Леондеса, перевод с английского. Издательство "Мир" 
Москва 1980. 
[44] . Шаврин В.В., Коналов А.С., Тисленко В.И, “Калибровка микроэлектроме-
ханических датчиков ускорений и угловых скоростей в бесплатформенных 
инерциальных навигационных системах”, Доклады ТУСУРа, № 1 (25), часть 
2, июнь 2012. 
107 
PHỤ LỤC 
Ph n m m mô đun lọc Kalman 
function [quatout,bias_out,V_inertial_out,euler_out,pqr_out,unfiltered_euler,bodyAcc_out,P_out,yaw1, gyro3, 
counter_out1, counter_out2, counter_out3] = fcn(gyros,accels,mags,dt_gps,gps_update,XYZ_inertial, time_05) 
%%************************************************************ 
%%***************** initialize variables ********************* 
%%************************************************************ 
persistent xhat; 
persistent P; 
persistent XYZ_inertial_prev; 
persistent XYZ_inertial_prev_prev; 
persistent bodyAcc; 
persistent V_inertial; 
persistent counter; 
persistent counter2; 
persistent counter3; 
persistent gyros_old; 
persistent out_test; 
persistent bodyV_prev; 
persistent bodyV; 
persistent bodyV_out; 
persistent bodyAcc_prev; 
persistent euler_out_prew; 
persistent euler_out_prew_prew; 
persistent accels_prew; 
persistent accels_prew_prew; 
persistent gyros_prew_prew; 
persistent gyros_prew; 
persistent gyros_tb; 
persistent mags_prew; 
persistent yaw_old_1; 
persistent yaw_old_1_prew; 
persistent yaw_old_1_prew_prew; 
persistent mags_prew_prew; 
persistent va; 
if isempty(counter) 
 counter = 0; 
108 
end 
if isempty(counter2) 
counter2 = 0; 
end 
if isempty(xhat) 
xhat =[0.999998858349755; 
 0.000871902307597; 
 0.000873425393729; 
 0.000871902307597; 
[1e-6;1e-6;1e-6];]; 
P=diag( [1^2*ones(1,3) .1^2*ones(1,3)] ); 
XYZ_inertial_prev = [0;0;0]; 
XYZ_inertial_prev_prev = [0;0;0]; 
euler_out_prew = [0;0;0]; 
euler_out_prew_prew =[0;0;0]; 
accels_prew = [0;0;0]; 
accels_prew_prew =[0;0;0]; 
gyros_prew_prew =[0;0;0]; 
gyros_prew =[0;0;0]; 
gyros_tb =[0;0;0]; 
mags_prew = [0;0;0]; 
mags_prew_prew =[0;0;0]; 
unfiltered_euler = [0;0;0]; 
bodyAcc = [0;0;0]; 
bodyAcc_prev = [0;0;0]; 
V_inertial = [0;0;0]; 
bodyV_out=[0;0;0]; 
bodyV_prev = [0;0;0]; 
bodyV = [0;0;0]; 
gps_alive = 0; 
gyros_old = gyros; 
%gyros_old(3) = gyros(3)-57.2*0.01745; 
yaw_old_1 = 0; 
gyro3 = 0; 
yaw_old_1_prew_prew = 0; 
yaw1=0; 
out_test =[0;0;0;0;0;0]; 
counter3 = 0; 
end 
dt = 0.01; % fundamental time step 
g = 9.81; 
109 
% B = [0.38848; 
% -0.00953; 
% 0.22781]; %NED 
B = [0.23092; 
0.00920; 
0.25392]; %NED 
a_n = 0.005; 
m_n = .005; 
% a_n = 50; 
% m_n = .5; 
Q_quat = 1e-2; 
Q_bias = 1e-2; 
tau_gyro = 100; 
exptau = exp(-.01/tau_gyro); 
R = diag([a_n^2*ones(1,3) m_n^2*ones(1,3)]); 
Q = diag([Q_quat^2*ones(1,3) dt*Q_bias^2*ones(1,3)]); 
x_old=xhat; 
%%************************************************************ 
%%******************** propogate state *********************** 
%%************************************************************ 
counter = counter+1; 
counter2 = counter2+1; 
counter3 = counter3+1; 
if accels(1)>0 
 accels(1)=accels(1)*0.94; 
% accels(1)=accels(1) - (2/104)*accels(1); 
end 
if accels(1)<0 
 accels(1)=accels(1)*0.97; 
% accels(1)=accels(1) + (1/93)*accels(1); 
end 
if accels(2)>0 
 accels(2)=accels(2)*0.96; 
% accels(2)=accels(2)-(2/104)*accels(2); 
end 
110 
if accels(2)<0 
 accels(2)=accels(2)*0.94; 
% accels(2)=accels(2)+(2/93)*accels(2); 
end 
if accels(3)>0 
 accels(3)=accels(3)*0.95; 
end 
if accels(3)<0 
 accels(3)=accels(3)*1; 
end 
 accels(1)=(accels(1)+ accels_prew(1)+ accels_prew_prew(1))/3; 
 accels(2)=(accels(2)+ accels_prew(2)+ accels_prew_prew(2))/3; 
 accels(3)=(accels(3)+ accels_prew(3)+ accels_prew_prew(3))/3; 
p = gyros_old(1)-x_old(5); 
q = gyros_old(2)-x_old(6); 
r = gyros_old(3)-x_old(7); 
normomega = norm([p;q;r]); 
normomegainv = inv(normomega); 
C = cos(.5*dt*normomega); 
Sn = sin(.5*dt*normomega)*normomegainv; 
Phi_old_small = [ C, -(p *Sn), -(q* Sn), -(r *Sn); 
(p *Sn), C, (r* Sn), -(q *Sn); 
(q* Sn), -(r *Sn), C, (p* Sn); 
x_minus=[Phi_old_small*x_old(1:4); %(predict state) 
exptau*x_old(5:7);]; 
if x_minus(1)<0 
%%************************************************************* 
%%************ propogate covariance *************************** 
%%************************************************************* 
%old state transition matrix 
111 
Phi_old = [1,0,0,-.5*dt, 0, 0; 
0,1,0, 0,-.5*dt, 0; 
0,0,1, 0, 0,-.5*dt; 
0,0,0,exptau, 0, 0; 
0,0,0, 0,exptau, 0; 
0,0,0, 0, 0,exptau;]; 
% propogate covariance 
P = Phi_old*(P+Q)*transpose(Phi_old); 
if gps_update 
 z= [accels; 
mags;]; 
q0=x_minus(1); 
q1=x_minus(2); 
q2=x_minus(3); 
q3=x_minus(4); 
Bx=B(1); 
By=B(2); 
Bz=B(3); 
% Expected measurement 
z_hat = [2*g*( q0*q2 - q1*q3 ); 
By*(-1 + 2*q0^2 + 2*q2^2) + 2*Bx*(q1*q2 - q0*q3) + 2*Bz*( q0*q1 + q2*q3); 
Bz*(-1 + 2*q0^2 + 2*q3^2) + 2*Bx*(q0*q2 + q1*q3) + 2*By*(-q0*q1 + q2*q3);]; 
% Linearized measurement matrix H 
H=[ 0, 2*g*(-1+2*q0^2+2*q3^2), -4*g*(q0*q1+q2*q3),0,0,0; 
1+2*q0^2+2*q2^2)+4*Bx*(q1*q2-q0*q3)+4*Bz*( q0*q1+q2*q3),0,0,0; 
4*Bx*(q0*q2+q1*q3)+4*By*(-q0*q1+q2*q3)+2*Bz*(-1+2*q0^2+2*q3^2), 0,-2*Bx*(-1+2*q0^2+2*q1^2)-
4*By*(q1*q2+q0*q3)-4*Bz*(-q0*q2+q1*q3),0,0,0; 
-2*By*(-1+2*q0^2+2*q2^2)-4*Bx*(q1*q2-q0*q3)-4*Bz*(q0*q1+q2*q3),2*Bx*(-
1+2*q0^2+2*q1^2)+4*By*(q1*q2+q0*q3)+4*Bz*(-q0*q2+q1*q3),0,0,0,0;]; 
K = P*transpose(H)*inv(H*P*transpose(H)+R); % kalman gain 
P = (eye(6)-K*H)*P; % update covariance 
xerror_hat = [zeros(3,1);x_old(5:7)]+K*(z-z_hat); % update state 
out_test = z-z_hat; 
xhat(5:7)=xerror_hat(4:6); 
xhat(1:4)=quatmult(x_minus(1:4),[1;xerror_hat(1:3)]); 
else 
z= [accels; 
mags;]; 
q0=x_minus(1); 
112 
q1=x_minus(2); 
q2=x_minus(3); 
q3=x_minus(4); 
Bx=B(1); 
By=B(2); 
Bz=B(3); 
% Expected measurement 
z_hat = [2*g*( q0*q2 - q1*q3 ); 
By*(-1 + 2*q0^2 + 2*q2^2) + 2*Bx*(q1*q2 - q0*q3) + 2*Bz*( q0*q1 + q2*q3); 
Bz*(-1 + 2*q0^2 + 2*q3^2) + 2*Bx*(q0*q2 + q1*q3) + 2*By*(-q0*q1 + q2*q3);]; 
% Linearized measurement matrix H 
H=[ 0, 2*g*(-1+2*q0^2+2*q3^2), -4*g*(q0*q1+q2*q3),0,0,0; 
0 ,-4*Bx*(q0*q2+q1*q3)-4*By*(-q0*q1+q2*q3)-2*Bz*(-1+2*q0^2+2*q3^2),2*By*(-
1+2*q0^2+2*q2^2)+4*Bx*(q1*q2-q0*q3)+4*Bz*( q0*q1+q2*q3),0,0,0; 
4*Bx*(q0*q2+q1*q3)+4*By*(-q0*q1+q2*q3)+2*Bz*(-1+2*q0^2+2*q3^2), 0,-2*Bx*(-1+2*q0^2+2*q1^2)-
4*By*(q1*q2+q0*q3)-4*Bz*(-q0*q2+q1*q3),0,0,0; 
-2*By*(-1+2*q0^2+2*q2^2)-4*Bx*(q1*q2-q0*q3)-4*Bz*(q0*q1+q2*q3),2*Bx*(-
1+2*q0^2+2*q1^2)+4*By*(q1*q2+q0*q3)+4*Bz*(-q0*q2+q1*q3),0,0,0,0;]; 
K = P*transpose(H)*inv(H*P*transpose(H)+R); % kalman gain 
P = (eye(6)-K*H)*P; % update covariance 
xerror_hat = [zeros(3,1);x_old(5:7)]+K*(z-z_hat); % update state 
out_test = z-z_hat; 
xhat(5:7)=xerror_hat(4:6); 
xhat(1:4)=quatmult(x_minus(1:4),[1;xerror_hat(1:3)]); 
end 
if xhat(1)<0 
xhat(1:4)=-xhat(1:4); 
end 
xhat(1:4) = xhat(1:4)/norm(xhat(1:4)); %normalize quaternion 
gyros_old = gyros; %save for next time 
%% *********************************************************************** 
%% ********************* unfiltered euler angles ************************* 
%% *********************************************************************** 
Gbody = accels; 
Gbody = Gbody/norm(Gbody)*.99999999; 
Pitch = asin(Gbody(1)); 
temp = -Gbody(2)/cos(Pitch); 
if abs(temp)<1 
Roll = asin(-Gbody(2)/cos(Pitch)); 
else 
Roll = pi/2*sign(temp); 
113 
end 
if sign(Gbody(3)) >= 0 % if upside down, correct phi 
Roll=-Roll-pi*sign(Gbody(2)); 
end 
ct = cos(Pitch); %set up trig for less computation time 
st = sin(Pitch); 
cp = cos(Roll); 
sp = sin(Roll); 
Bflat = [ct, st*sp, cp*st; 
0, cp, -sp;]*mags; %rotate mags into phi=0, theta=0 
Yaw = 0.2686 -atan2g(Bflat(2),Bflat(1)); %calculate psi 
unfiltered_euler = [Pitch;Roll;Yaw]; 
%% *********************************************************************** 
%% ********************* outputs ***************************************** 
%% *********************************************************************** 
P_out = P; 
quatout = xhat(1:4); 
euler_out = 57.3*quat2eul(quatout); 
% LAY TRUNG BINH THEO DO 
euler_out(3)= (euler_out(3)+ euler_out_prew(3)+euler_out_prew_prew(3))/3 ; 
euler_out_prew_prew(3) = euler_out_prew(3); 
euler_out_prew(3) = euler_out(3); 
euler_out(2)= (euler_out(2)+ euler_out_prew(2)+euler_out_prew_prew(2))/3; 
euler_out_prew_prew(2) = euler_out_prew(2); 
euler_out_prew(2) = euler_out(2); 
euler_out(1)= (euler_out(1)+ euler_out_prew(1)+euler_out_prew_prew(1))/3; 
euler_out_prew_prew(1) = euler_out_prew(1); 
euler_out_prew(1) = euler_out(1); 
%TRA VE RADIAN 
euler_out = euler_out/57.3; 
114 
% euler_out(1) = -euler_out(1); %reverse sign for GS 
bias_out = xhat(5:7); 
pqr_out = gyros-xhat(5:7); 
unfiltered_euler = [Roll; Pitch; Yaw]; 
bodyAcc_out = accels; 
V_inertial_out = bodyV; 
accels_prew_prew(1) = accels_prew(1); 
accels_prew(1) = accels(1); 
accels_prew_prew(2) = accels_prew(2); 
accels_prew(2) = accels(2); 
accels_prew_prew(3) = accels_prew(3); 
accels_prew(3) = accels(3); 
mags_prew_prew(1) = mags_prew(1); 
mags_prew(1) = mags(1); 
mags_prew_prew(2) = mags_prew(2); 
mags_prew(2) = mags(2); 
mags_prew_prew(3) = mags_prew(3); 
mags_prew(3) = mags(3); 
if(yaw1>180) 
 yaw1=yaw1-360; 
end 
if(yaw1<-180) 
 yaw1= yaw1 + 360; 
end 
yaw_old_1 = yaw1; 
gyro3 = gyros_old(3); 
counter_out2 = counter2; 
 if counter2 == 700 
115 
 counter2 = 0; 
% yaw1 = euler_out(3); 
% yaw_old_1 = euler_out(3); 
 end 
counter_out3 = counter3; 
 if counter3 == 50 
 counter3 = 0; 
 end 
counter_out1 = counter; 
 if counter == 10; 
 counter = 0; 
 end 
end 
%================================================================ 
%================== additional functions ====================== 
%================================================================ 
function quat = quatmult(q1,q2) 
% Multiplies two quaternions. 
qv1 = q1(2:4); 
qs1 = q1(1); 
qv2 = q2(2:4); 
qs2 = q2(1); 
quatv = cross(qv1,qv2) + qs1*qv2 + qs2*qv1; 
quats = qs1*qs2 - dot(qv1,qv2); 
quat = [quats;quatv]; 
end 
function quat = quatinv(q) 
% Inverts a quaternion. 
quat = [q(1);-q(2:4)]; 
end 
function angles = quat2eul( q ) 
qin = q./norm(q); 
y=2.*(qin(3,1).*qin(4,1) + qin(1,1).*qin(2,1)); 
x=qin(1,1).^2 - qin(2,1).^2 - qin(3,1).^2 + qin(4,1).^2; 
phi = atan2g(y,x); 
theta = asin(-2.*(qin(2,1).*qin(4,1) - qin(1,1).*qin(3,1))); 
116 
y=2.*(qin(2,1).*qin(3,1) + qin(1,1).*qin(4,1)); 
x=qin(1,1).^2 + qin(2,1).^2 - qin(3,1).^2 - qin(4,1).^2; 
psi = atan2g(y,x); 
angles = [phi; theta; psi]; 
end 
function angle = atan2g(y,x) 
angle = 0.; 
if x>0 
angle = atan(y/x); 
end 
if x==0 
angle = .5*pi*sign(y); 
end 
if x<0 
angle = pi*sign(y)+atan(y/x); 
end 
end 
function q = eul2quat(angles) 
% converts phi,theta,phi into q 
% lifted straight from matlab's function (with a transposed output): 
cang = cos( angles/2 ); 
sang = sin( angles/2 ); 
q = [ cang(:,1).*cang(:,2).*cang(:,3) + sang(:,1).*sang(:,2).*sang(:,3); 
sang(:,1).*cang(:,2).*cang(:,3) - cang(:,1).*sang(:,2).*sang(:,3); 
cang(:,1).*sang(:,2).*cang(:,3) + sang(:,1).*cang(:,2).*sang(:,3); 
cang(:,1).*cang(:,2).*sang(:,3) - sang(:,1).*sang(:,2).*cang(:,3)]; 
end 
function dcm = q2dcm( q ) 
%qin = q./norm(q); 
% The direction cosine matrix performs the coordinate 
% transformation of a vector in inertial axes to a vector in body axes. 
qin = q; % don't need to normalize because it is already done 
%qin = quatnormalize( q ); 
dcm = zeros(3,3); 
dcm(1,1) = qin(1).^2 + qin(2).^2 - qin(3).^2 - qin(4).^2; 
dcm(1,2) = 2.*(qin(2).*qin(3) + qin(1).*qin(4)); 
dcm(1,3) = 2.*(qin(2).*qin(4) - qin(1).*qin(3)); 
dcm(2,1) = 2.*(qin(2).*qin(3) - qin(1).*qin(4)); 
dcm(2,2) = qin(1).^2 - qin(2).^2 + qin(3).^2 - qin(4).^2; 
dcm(2,3) = 2.*(qin(3).*qin(4) + qin(1).*qin(2)); 
dcm(3,1) = 2.*(qin(2).*qin(4) + qin(1).*qin(3)); 
117 
dcm(3,2) = 2.*(qin(3).*qin(4) - qin(1).*qin(2)); 
dcm(3,3) = qin(1).^2 - qin(2).^2 - qin(3).^2 + qin(4).^2; 
end 
function [Pitch, Roll, Yaw] = accelMagsBody2euler(acc,mags,body) 
 Gbody = acc-body; 
 Gbody = Gbody/norm(Gbody)*.99999999; 
 Pitch = asin(Gbody(1)); 
 temp = -Gbody(2)/cos(Pitch); 
 if abs(temp)<1 
 Roll = asin(-Gbody(2)/cos(Pitch)); 
 else 
 Roll = pi/2*sign(temp); 
 end 
 if sign(Gbody(3)) >= 0 % if upside down, correct phi 
 Roll=-Roll-pi*sign(Gbody(2)); 
 end 
 ct = cos(Pitch); %set up trig for less computation time 
 st = sin(Pitch); 
 cp = cos(Roll); 
 sp = sin(Roll); 
 Bflat = [ct, st*sp, cp*st; 
 0, cp, -sp;]*mags; %rotate mags into phi=0, theta=0 
 Yaw = 0.2686 -atan2g(Bflat(2),Bflat(1)); %calculate psi 
end