Luận án Cải tiến hệ thống định vị quán tính nhằm nâng cao độ chính xác ước lượng thông số bước đi trong chăm sóc sức khỏe

ó: 
𝑄𝑘 = (𝑄 + 𝐴𝑟𝑄 + 𝑄𝐴
𝑇𝑟 + 𝐴𝑄𝐴𝑇𝑟2)𝑑𝑟
𝑇
0
 (2-28) 
Khai triển tích phân cho (2-28), ta có: 
 Trang 60 
𝑄𝑘 = 𝑄𝑇 + (𝐴 + 𝐴
𝑇)𝑄
𝑇2
2
+ 𝐴𝑄𝐴𝑇
𝑇3
3
 (2-29) 
Quá trình dự đoán theo mô hình của bộ lọc Kalman gồm các bước như sau: 
- Tính các ma trận: 𝐴𝑘 theo (2-25), 𝑄𝑘 theo (2-28) 
- Dự đoán trạng thái mới: 
𝑥𝑘+1
− = 𝐴𝑘𝑥𝑘 (2-30) 
- Dự đoán hiệp phương sai cho bộ lọc Kalman: 
𝑃𝑘+1
− = 𝐴𝑘𝑃𝑘𝐴𝑘
𝑇 + 𝑄𝑘 (2-31) 
Phương trình cập nhật tổng quát cho bộ lọc Kalman có dạng: 
𝑧𝑘 = 𝐻𝑘𝑥𝑘 + 𝑣𝑧𝑘 (2-32) 
trong đó: 
- 𝑧𝑘 là ma trận phép đo, 
- 𝐻𝑘 là ma trận quan sát và 𝑣𝑧𝑘 đại diện cho nhiễu trắng của phép đo với 
ma trận hiệp phương là 𝑅𝑘 tại thời điểm 𝑘. 
Tùy vào đối tượng, đặc trưng chuyển động và tình huống cụ thể mà xây dựng 
các phương trình cập nhật phù hợp để nâng cao độ chính xác trong ước lượng chuyển 
động. Đây là phần đóng góp chính của luận án sẽ được trình bày chi tiết trong Chương 
3 và Chương 4. 
Quá trình cập nhật giá trị đo gồm các bước như sau: 
- Tính các ma trận 𝐻𝑘 và 𝑅𝑘 
- Tính hệ số Kalman 
𝐾𝑘 = 𝑃𝑘
−𝐻𝑘
𝑇(𝐻𝑘𝑃𝑘
−𝐻𝑘
𝑇 + 𝑅𝑘)
−1 (2-33) 
- Cập nhật giá trị ước lượng sử dụng giá trị đo 
𝑥𝑘 = 𝑥𝑘
− + 𝐾𝑘(𝑧𝑘 − 𝐻𝑘𝑥𝑘
−) (2-34) 
- Cập nhật hiệp phương sai 
𝑃𝑘 = (𝐼 − 𝐾𝑘𝐻)𝑃𝑘
− (2-35) 
 Trang 61 
Hình 2.11 Hoạt động của bộ lọc Kalman 
+
-
+
BỘ LỌC 
KALMAN
INA
�̂� 
𝑣 
�̅� 
�̂� 
�̅� 
�̅� 
𝑟 
𝑣 
𝑞 
CB 
gia tốc 
CB vận 
tốc góc
IMU
-
HIỆU 
CHỈNH
𝑞 = �̂� ⊗ [
1
�̅�
] 
𝑟 = �̂� + �̅� 
𝑣 = 𝑣 + �̅� 
𝜔 
 𝜔 
�̂�0 
𝑣0 
 [𝑎]𝑤 
[𝑎]𝑤 
𝑦𝑎 
𝑏𝑔 
𝑏𝑎 
𝑏𝑎 
Chuyển 
sang 
WCS 
+
-
[�̃�]𝑤 
 𝑣 
�̂�0 
�̂� 
𝑦𝑔 
Các phép đo 
𝑏𝑔 
𝑣 = 𝑣 
�̂� = 𝑟 
�̂� = 𝑞 
Các tham số 
�̂� 𝑣 �̂� 
Hình 2.12 Thuật toán của hệ thống INS sử dụng bộ lọc Kalman MEKF 
Do bộ lọc MEKF là bộ lọc gián tiếp ước lượng các sai số �̅�, �̅�, �̅� nên sai số tại 
thời điểm ban đầu bằng không và khởi tạo giá trị ban đầu cho biến trạng thái là 𝑥0 =
015×1. Việc khởi tạo này là hoàn toàn chính xác nên hiệp phương sai tương ứng của 
chúng bằng 0, riêng thành phần 𝑏𝑎 và 𝑏𝑔 thì thành phần hiệp phương sai của chúng 
Tính các ma trận 𝐴𝑘, 𝑄𝑘 
Dự đoán trạng thái mới 
𝑥𝑘+1
− = 𝐴𝑘𝑥𝑘 
Dự đoán hiệp phương sai 
𝑃𝑘+1
− = 𝐴𝑘𝑃𝑘𝐴𝑘
𝑇 + 𝑄𝑘 
Cập nhật theo thời gian 
(dự đoán theo mô hình) 
Tính các ma trận 𝐻𝑘, 𝑅𝑘 
Tính hệ số Kalman 
𝐾𝑘 = 𝑃𝑘
−𝐻𝑇(𝐻𝑃𝑘
−𝐻𝑇 + 𝑅)−1 
Cập nhật sử dụng giá trị đo 𝑧𝑘 
𝑥𝑘 = 𝑥𝑘
− + 𝐾𝑘(𝑧𝑘 − 𝐻�̂�𝑘
−) 
Cập nhật hiệp phương sai 
𝑃𝑘 = (𝐼 − 𝐾𝑘𝐻)𝑃𝑘
− 
Cập nhật giá trị đo 
(hiệu chỉnh) 
Khởi tạo giá trị 𝑥0
−, 𝑃0
− 
 Trang 62 
ban đầu bằng với ma trận hiệp phương sai 𝑄𝑏𝑎 và 𝑄𝑏𝑔. Do vậy, khởi tạo giá trị ban 
đầu của hiệp phương sai 𝑃0 như sau: 
𝑃0 =
[
03×3 03×3 03×3 03×3 03×3
03×3 𝑄𝑏𝑔 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 𝑄𝑏𝑎 ]
 (2-36) 
Tóm lại, hoạt động của bộ lọc Kalman được tóm tắt như trong Hình 2.11 và 
hoạt động chung của thuật toán hệ thống INS sử dụng bộ lọc Kalman kiểu MEKF 
được minh hoạ chi tiết trong Hình 2.12. 
2.4 Kết luận chương 
 Trong chương này đã trình bày về cấu tạo, nguyên lý của cảm biến IMU và 
việc triển khai thuật toán hệ thống INS. Trong đó, thuật toán hệ thống INS sử dụng 
bộ lọc Kalman kiểu MEKF với các vấn đề chính như sau: 
- Triển khai thuật toán INA bằng cách kết hợp tích phân của các tín hiệu 
vận tốc góc và gia tốc từ cảm biến để ước lượng sơ bộ hướng, vận tốc và 
vị trí của cảm biến IMU trong quá trình chuyển động. 
- Triển khai bộ lọc Kalman kiểu MEKF sử dụng mô hình sai số quán tính 
để ước lượng sai số của các giá trị hướng, vận tốc và vị trí sơ bộ từ thuật 
toán INA, sau đó hiệu chỉnh lại giá trị của chúng. Các dữ liệu gia tốc và 
vận tốc góc của cảm biến IMU và các giá trị ước lượng sơ bộ của thuật 
toán INA được đưa vào bộ lọc để xây dựng tham số của bộ lọc Kalman. 
- Các giá trị đo từ các cảm biến khác hoặc từ một số giá trị suy ra từ tính 
chất chuyển động được dùng để xây dựng các phương trình cập nhật cho 
bộ lọc Kalman. 
Thuật toán hệ thống INS sử dụng bộ lọc Kalman trong chương này sẽ được 
nghiên cứu ứng dụng vào hệ thống INS đặt trên bàn chân ở Chương 3 và hệ thống 
INS đặt trên khung tập đi ở Chương 4. Trong đó, mô hình của bộ lọc sẽ được điều 
chỉnh, các phương trình cập nhật sẽ được xây dựng phù hợp với từng hệ thống nhằm 
nâng cao độ chính xác của việc ước lượng chuyển động. 
 Trang 63 
Chương 3. NGHIÊN CỨU XÂY DỰNG HỆ THỐNG ĐỊNH VỊ 
QUÁN TÍNH ĐẶT TRÊN BÀN CHÂN 
3.1 Giới thiệu chương 
Trong chương này, đề xuất hệ thống INS đặt trên bàn chân nhằm nâng cao độ 
chính xác cho việc ước lượng chuyển động cũng như thông số bước đi. Hệ thống INS 
đề xuất này gồm 01 cảm biến IMU kết hợp với 01 cảm biến khoảng cách đặt trên bằn 
chân. Việc gắn hệ thống INS trên bàn chân với mục đích sử dụng cập nhật ZUPT 
trong khi cảm biến khoảng cách hướng xuống mặt đất trong quá trình bước đi là để 
tham chiếu độ cao và cập nhật chuyển động cho bàn chân. 
Một số vấn đề cần giải quyết trong chương này gồm: 
- Ước lượng chính xác mối quan hệ gồm vị trí và hướng của cảm biến 
khoảng cách trong hệ toạ độ của IMU. 
- Xây dựng mô hình bộ lọc Kalman kiểu MEKF cho hệ thống INS mới 
dựa mô hình bộ lọc Kalman cho hệ thống INS cơ bản đã được xây dựng 
ở Chương 2. 
- Xây dựng phương trình cập nhật ZUPT và các phương trình cập nhật sử 
dụng dữ liệu từ cảm biến khoảng cách. 
- Trích xuất thông số bước đi từ quỹ đạo chuyển động của cảm biến IMU 
đặt trên bàn chân. 
- Đánh giá hiệu quả hệ thống đề xuất. 
Nội dung chương này đã được chính tác giả và cộng sự công bố trong bài báo 
[50] trên tạp chí Sensors (SCIE, Q1) năm 2015 và bài báo [99] trong kỷ yếu hội nghị 
VCCA năm 2019. Trong bài báo [50], các tác giả đã gắn 2 cảm biến khoảng cách vào 
hệ thống INS đặt trên bàn chân và sử dụng bộ lọc Kalman 21 trạng thái để ước lượng 
chuyển động của bàn chân. Trong bài báo [99], các tác giả chỉ sử dụng 1 cảm biến 
khoảng cách và bộ lọc Kalman 15 trạng thái nhằm giảm khối lượng tính toán nhưng 
vẫn đảm bảo độ chính xác của hệ thống. 
 Trang 64 
3.2 Đề xuất hệ thống INS đặt trên bàn chân 
Bàn chân là điểm thích hợp để đặt cảm biến IMU do chuyển động bước chân 
có tính chu kỳ lặp đi lặp lại cũng như luôn tồn tại một khoảng thời gian bàn chân 
chạm đất ở đầu và cuối mỗi bước đi. Tại thời điểm bàn chân chạm đất thì vận tốc có 
thể được xem là bằng không (nên khoảng thời gian này gọi là ZVI), độ cao bàn chân 
cũng có thể được xem là bằng không trong điều kiện bước đi trên mặt phẳng. Các 
thông tin vận tốc và độ cao bàn chân tại khoảng thời gian ZVI được sử dụng phổ biến 
như là một tín hiệu đo để loại bỏ sai số và cập nhật vận tốc, vị trí và hướng bàn chân 
trong bộ lọc Kalman (thường gọi là ZUPT). Ngoài ra, do cảm biến IMU được đặt trên 
bàn chân nên chuyển động của cảm biến IMU trong quá trình bước đi phản ánh quá 
trình chuyển động của bàn chân do đó các thông số bước đi dễ dàng trích xuất được 
từ quỹ đạo chuyển động của cảm biến IMU. 
Trong chương này, luận án sử dụng cảm biến khoảng cách hỗ trợ hệ thống INS 
gắn trên bàn chân để nâng cao độ chính xác trong ước lượng vị trí và hướng bàn chân. 
Cảm biến khoảng cách được bố trí hướng xuống mặt đất trong quá trình di chuyển 
nhằm nâng cao độ chính xác của hệ thống INS. Mô hình bộ lọc MEKF trong Chương 
2 cũng được điều chỉnh để ước lượng vị trí và hướng của cảm biến khoảng cách trong 
hệ toạ độ BCS. Từ vị trí và hướng của cảm biến khoảng cách, thông tin về độ cao của 
bàn chân được xác định từ giá trị đo của cảm biến khoảng cách. Điều này cho phép 
xây dựng phương trình cập nhật của bộ lọc MEKF để cập nhật quỹ đạo chuyển động 
cho bàn chân trong quá trình di chuyển nhằm nâng cao độ chính xác trong ước lượng 
các thông số bước đi. 
Hệ thống đề xuất được thể hiện trong Hình 3.1 bao gồm 01 IMU có tần số lấy 
mẫu được cấu hình là 100 𝐻𝑧 và 01 cảm biến khoảng cách được gắn cố định trên một 
chiếc giày. Các thông số được quan tâm của cảm biến khoảng cách là giới hạn đo và 
tần số hoạt động. Trong đó, cảm biến khoảng cách dùng để đo độ cao của bàn chân 
trong quá trình bước đi, độ cao này thường nhỏ hơn 20 𝑐𝑚. Thời gian di chuyển của 
bàn chân trong một bước đi thường lớn hơn 30 𝑚𝑠. Trong khoảng thời gian này cần 
phải có ít nhất một dữ liệu từ cảm biến khoảng cách để cập nhật. Do vậy tần số của 
 Trang 65 
cảm biến khoảng cách yêu cầu phải lớn hơn 33 𝐻𝑧. Trong luận án này chọn cảm biến 
khoảng cách VL6180 với khoảng cách đo lên đến 20 mm, sai số khoảng 2 mm và tần 
số lên đến 50 𝐻𝑧. Việc đồng bộ các tín hiệu từ các cảm biến được thực hiện bằng 
cách đọc xen kẽ 01 dữ liệu từ cảm biến khoảng cách với 03 dữ liệu từ cảm biến IMU. 
Do vậy, cảm biến khoảng cách được cấu hình hoạt động ở tần số 33,33 𝐻𝑧. 
IMU
 D br
 D bn
Cảm biến 
khoảng cách 
Hình 3.1 Tổng quan về INS đặt trên bàn chân (nguồn [50]) 
Thuật toán của hệ thống INS sử dụng bộ lọc MEKF trong Chương 2 được sử 
dụng để ước lượng hướng 𝑞, vận tốc 𝑣 và vị trí 𝑟 theo thời gian của bàn chân (được 
đại diện bởi hệ toạ độ BCS). Tuy nhiên, mô hình của bộ lọc MEKF có sự thay đổi 
bằng việc đưa vị trí và hướng của cảm biến khoảng cách trong BCS vào bộ lọc để 
ước lượng (do khó có thể đo chính xác bằng thước), đồng thời các thành phần nhiễu 
chậm thay đổi 𝑏𝑎 , 𝑏𝑔 trong Hình 2.12 được loại khỏi bộ lọc để giảm khối lượng tính 
toán. Lúc này, 𝑏𝑔 được tính sơ bộ bằng giá trị thu thập tại thời điểm cảm biến IMU 
đứng yên ban đầu và 𝑏𝑎 = 03×1 trong thuật toán INA ở Hình 2.12. 
Ngoài ra, các phương trình cập nhật của bộ lọc Kalman cần được xây dựng để 
sử dụng các tín hiệu đo từ cảm biến khoảng cách cũng như thông tin bàn chân chạm 
đất để cập nhật sai số của bộ lọc. Do vậy, trong chương này chỉ đề cập đến việc xây 
dựng mô hình bộ lọc và các phương trình cập nhật cho bộ lọc có kế thừa kết quả từ 
Chương 2. Các thông số bước đi sẽ được trích xuất từ vị trí 𝑟 kết hợp với các thời 
 Trang 66 
điểm bàn chân chạm đất. Trong chương này sử dụng hệ trục toạ độ vật thể BCS và 
toàn cục WCS đã được trình bày trong Mục 2.2.2.1. 
3.3 Xây dựng mô hình bộ lọc MEKF cho hệ thống 
Gọi [𝑟𝐷]𝑏 và [𝑛𝐷]𝑏 là vị trí và hướng của cảm biến khoảng cách trong hệ tọa 
độ BCS. Lúc này ta có: 
𝑟𝐷 = �̂�𝐷 + �̅�𝐷
𝑛𝐷 = �̂�𝐷 + �̅�𝐷
 (3-1) 
trong đó: 
- �̂�𝐷 và �̂�𝐷 là giá trị ước lượng sơ bộ của thông số vị trí và hướng của cảm 
biến khoảng cách 
- �̅�𝐷 và �̅�𝐷 là các giá trị sai số của vị trí và hướng. Trong phần thí nghiệm 
cảm biến khoảng cách có 2 vị trí đặt với thông số như sau 
o Vị trí 1: �̂�𝐷 = [ −0,008 ; 0,080 ; 0 ], �̂�𝐷 = [1 0 0 ]′ 
o Vị trí 2: �̂�𝐷 = [ −0,006 ; −0,062 ; 0 ], �̂�𝐷 = [ 1 0 0 ]′ 
Việc đo sơ bộ �̂�𝐷 và �̂�𝐷 bằng thước không đạt được độ chính xác cao nên sai 
số của chúng (�̅�𝐷, �̅�𝐷) được đưa vào bộ lọc Kalman để ước lượng nhằm nâng cao độ 
chính xác. 
Như vậy, các sai số �̅�, �̅�, 𝑟,̅ �̅�𝐷 và �̅�𝐷 được đưa vào bộ lọc Kalman để ước 
lượng nên các biến trạng thái được sử dụng trong bộ lọc Kalman như sau: 
𝑥 =
[
�̅�
�̅�
�̅�
�̅�𝐷
�̅�𝐷]
∈ 𝑅15 (3-2) 
Mô hình cho bộ lọc Kalman có dạng như trong (2-19). Trong đó, thành phần 
𝑏𝑔 và 𝑏𝑎 được bỏ qua khi tính �̇̅�, �̇̅� và �̇̅�. Tổng hợp kết quả ta có: 
 Trang 67 
�̇̅� = [−𝑦𝑔 ×]�̅� −
1
2
𝑣𝑔
�̇̅� = �̅�
 �̇̅� ≈ −2𝐶𝑇(�̂�)𝐾(𝑦𝑎)�̅� − 𝐶
𝑇(𝑞)𝑣𝑎
�̇̅�𝐷 = 0
�̇̅�𝐷 = 0
 (3-3) 
trong đó, �̅�𝐷 = 𝑟𝐷 − �̂�𝐷 và �̅�𝐷 = 𝑛𝐷 − �̂�𝐷. Do thành phần 𝑟𝐷 , �̅�𝐷 , 𝑛𝐷 và �̅�𝐷 là các hằng 
số nên �̇̅�𝐷 = 0 và �̇̅�𝐷 = 0. 
Như vậy các ma trận trong mô hình hệ thống đề xuất như sau: 
𝐴 =
[
[−𝑦𝑔 ×] 03×3 03×3 03×3 03×3
03×3 03×3 𝐼3 03×3 03×3
−2𝐶𝑇(�̂�)[𝑦𝑎 ×] 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3]
, 𝑤 =
[
 −
1
2
𝑣𝑔
0
−𝐶𝑇(�̂�)𝑣𝑎
0
0 ]
Ma trận hiệp phương sai của nhiễu mô hình được tính tương tự như trong 
(2-22), trong đó: 
- 𝑄(1: 3,1: 3) = 𝐸{𝑤(1: 3)𝑇𝑤(1: 3)} =
1
4
𝑅𝑔 
- 𝑄(4: 6,4: 6) = 𝐸{𝑤(4: 6)𝑇𝑤(4: 6)} = 0 
- 𝑄(7: 9,7: 9) = 𝐸{𝑤(7: 9)𝑇𝑤(7: 9)} = 𝐶𝑇(�̂�)𝑅𝑎𝐶(�̂�) 
- 𝑄(10: 12,10: 12) = 𝐸{𝑤(10: 12)𝑇𝑤(10: 12)} = 0 
- 𝑄(13: 15,13: 15) = 𝐸{𝑤(13: 15)𝑇𝑤(13: 15)} = 0 
Lúc này ta có: 
𝑄 =
[
1
4
𝑅𝑔 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 𝐶
𝑇(�̂�)𝑅𝑎𝐶(�̂�) 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3]
 (3-4) 
trong đó 𝑅𝑎 và 𝑅𝑔 được tính trong (2-20). 
Tương tự như đã trình bày ở Chương 2, mô hình của bộ lọc ở dạng rời rạc như 
sau: 
 Trang 68 
𝑥𝑘+1 = 𝐴𝑘𝑥𝑘 + 𝑤𝑘 (3-5) 
trong đó: 
𝐴𝑘 = 𝑒
𝐴𝑇 
𝑤𝑘 = 𝑒
𝐴𝑟𝑤𝑑𝑟
𝑇
0
Trong khoảng thời gian lấy mẫu 𝑇, các thành phần trong ma trận 𝐴 là hằng số. 
Do vậy ta có: 
𝑒𝐴𝑇 ≈ 𝐼 + 𝐴𝑇 +
1
2
𝐴2𝑇2 (3-6) 
trong đó: 
𝐴 =
[
𝐴1 03×3 03×3 03×3 03×3
03×3 03×3 𝐼3 03×3 03×3
𝐴2 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3]
𝐴2 =
[
𝐴1
2 03×3 03×3 03×3 03×3
𝐴2 03×3 03×3 03×3 03×3
𝐴2𝐴1 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3
03×3 03×3 03×3 03×3 03×3]
𝐴1 = [−𝑦𝑔 ×]; 𝐴2 = −2𝐶
𝑇(�̂�)[𝑦𝑎 ×] 
Như vậy 𝐴𝑘 được xấp xỉ như sau 
𝐴𝑘 ≈
[
 𝐼3 + 𝐴1𝑇 +
𝐴1
2𝑇2
2
03×3 03×3 03×3 03×3
𝐴2
𝑇2
2
𝐼3 𝐼3𝑇 03×3 03×3
𝐴2𝑇 + 𝐴2𝐴1
𝑇2
2
03×3 𝐼3 03×3 03×3
03×3 03×3 03×3 𝐼3 03×3
03×3 03×3 03×3 03×3 𝐼3 ]
 (3-7) 
Các ma trận 𝑄𝑘 và 𝑃𝑘+1
− được xác định như trong công thức (2-29) và (2-31). 
 Trang 69 
3.4 Xây dựng các phương trình cập nhật cho bộ lọc Kalman 
3.4.1 Cập nhật vận tốc ZUPT 
Trong quá trình bàn chân bước đi, luôn tồn tại các khoảng thời gian ZVI sau 
mỗi bước đi. Lúc này vận tốc của bàn chân có thể xem bằng không và độ cao của bàn 
chân cũng được xem là bằng không. 
Trong [100], các ZVI này có thể được phát hiện trực tiếp bởi cảm biến vận tốc 
Doppler. Tuy nhiên, chúng ta có thể phát hiện các ZVI gián tiếp bằng cách sử dụng 
thuật toán phát hiện ZVI [101], [102]. 
Trong luận án này, một thuật toán phát hiện ZVI đơn giản được sử dụng. Nếu 
những điều kiện dưới đây được thỏa mãn thì thời điểm rời rạc 𝑚 phải thuộc ZVI [93], 
[50]: 
‖𝑦𝑔,𝑖‖ ≤ 𝐵𝑔, 𝑚 −
𝑁𝑔
2
≤ 𝑖 ≤ 𝑚 +
𝑁𝑔
2
‖𝑦𝑎,𝑖 − 𝑦𝑎,𝑖−1‖ ≤ 𝐵𝑎 , 𝑚 −
𝑁𝑎
2
≤ 𝑖 ≤ 𝑚 +
𝑁𝑎
2
 (3-8) 
trong đó: 
- 𝐵𝑔, 𝐵𝑎 là các giá trị ngưỡng đặt trước cho độ lớn được xem là chuyển 
động của giá trị vận tốc góc và độ thay đổi của gia tốc 
- 𝑁𝑔 và 𝑁𝑎 là một số nguyên đại diện cho khoảng thời gian đủ lớn để xem 
là đứng yên. Trong luận án này, chọn 𝑁𝑔 = 𝑁𝑎 = 30 tương ứng với 
khoảng thời gian là 0,3 giây, 𝐵𝑔 = 0,2 𝑟𝑎𝑑/𝑠 và 𝐵𝑎 = 0,4 𝑚/𝑠
2 . 
Những giá trị này được lấy từ thực nghiệm thông qua việc phân tích dữ 
liệu liên quan trong quá trình bước đi. 
Tại các thời khoảng thời gian ZVI này, ta có: 
𝑣 = 03×1
𝑟𝑧 = 0
 (3-9) 
trong đó 𝑟 = [𝑟𝑥 𝑟𝑦 𝑟𝑧]𝑇 . 
Từ 𝑣 = 𝑣 + �̅� và 𝑟 = �̂� + �̅� kết hợp với (3-9), ta có: 
 Trang 70 
�̅� = −𝑣
�̅�𝑧 = −�̂�𝑧
 (3-10) 
Vậy phương trình cập nhật cho bộ lọc Kalman như sau: 
𝑧𝑍𝑉𝐼 = 𝐻𝑍𝑉𝐼𝑥 + 𝑣𝑍𝑉𝐼 (3-11) 
trong đó: 
- 𝑧𝑍𝑉𝐼 = [
𝑣
�̂�𝑧
] 
- 𝐻𝑍𝑉𝐼 = [
03×6 𝐼3 03×6
01×5 1 03×3 03×6
] 
- 𝑣𝑍𝑉𝐼 = [
𝑣𝑣
𝑣𝑟𝑧
] 
- 𝑅𝑍𝑉𝐼 = [
𝑅𝑣 03×1
01×3 𝑅𝑟𝑧
] 
- 𝑣𝑣 = 𝑁(0, 𝑅𝑣) và 𝑣𝑟𝑧 = 𝑁(0, 𝑅𝑟𝑧) là nhiễu trắng thể hiện việc vận tốc và 
độ cao của bàn chân không hoàn toàn bằng không. Trong đó, tốc độ rung 
động của bàn chân khi đứng trên mặt đất khoảng 0,01 𝑚/𝑠 và độ dao 
động về độ cao bàn chân khoảng 0,01 𝑚. Trong luận án chọn 𝑅𝑣 =
0,0001𝐼3 (
𝑚2
𝑠2
) , 𝑅𝑟𝑧 = 0,0001 𝑚
2. 
3.4.2 Xây dựng phương trình cập nhật sử dụng cảm biến khoảng cách 
Giá trị đo của cảm biến khoảng cách 𝑧𝐷 có thể được mô hình hóa như sau: 
𝑧𝐷 = 𝑑𝐷 + 𝑣𝐷 (3-12) 
trong đó: 
- 𝑑𝐷 là giá trị khoảng cách từ cảm biến khoảng cách đến điểm mà cảm 
biến chỉ vào trên mặt đất, 
- 𝑣𝐷 = 𝑁(0, 𝑅𝐷) là nhiễu trắng của cảm biến khoảng cách. Trong quá 
trình thực nghiệm xác nhận độ phân giải của cảm biến là 1 𝑚𝑚 và sai số 
của cảm biến là ±2 𝑚𝑚 nên luận án chọn 𝑅𝐷 = 0,000004 𝑚
2. 
 Trang 71 
Chú ý rằng đây không phải là độ cao của cảm biến vì cảm biến không hướng 
vuông góc với mặt đất trong quá trình bước đi. Độ cao này có thể tính được dựa vào 
phương của cảm biến khoảng cách. 
Do mặt đất bằng phẳng và gốc hệ trục tọa độ WCS nằm trên mặt đất lúc này 
độ cao của bàn chân được tính như sau: 
[0 0 1]𝑟 = −[0 0 1]𝐶𝑇(𝑞)[𝑟𝐷 + 𝑛𝐷𝑑𝐷]𝑏 (3-13) 
trong đó, [𝑟𝐷 + 𝑛𝐷𝑑𝐷]𝑏 chính là vectơ được xác định từ cảm biến IMU đến điểm mà 
cảm biến khoảng cách chỉ trên mặt đất xét trong hệ toạ độ BCS, vectơ này được biểu 
diễn trong hệ toạ độ WCS bằng cách nhân với ma trận quay từ BCS sang WCS như 
sau 𝐶′(𝑞)[𝑟𝐷 + 𝑛𝐷𝑑𝐷]𝑏. 
Sử dụng tính chất 𝐶𝑇(𝑞) = 𝐶𝑇(�̂�)(𝐼3 + 2[�̅� ×]) [90] để triển khai phương 
trình (3-13), ta có: 
[0 0 1]𝑟 = −[0 0 1]𝐶𝑇(�̂�)(𝐼3 + 2[�̅� ×]) [𝑟𝐷 + 𝑛𝐷𝑑𝐷]𝑏 (3-14) 
Thay (3-1) và (3-12) vào (3-14), ta có: 
[0 0 1](�̂� + �̅�) = 
−[0 0 1]𝐶𝑇(�̂�)(𝐼 + 2[�̅� ×]) [�̂�𝐷 + �̅�𝐷 + (�̂�𝐷 + �̅�𝐷)(𝑧𝐷 − 𝑣𝐷)]𝑏 
(3-15) 
Các giá trị sai số như �̅�, �̅�𝐷 , �̅�𝐷 , �̅� và thành phần nhiễu đo khoảng cách 𝑣𝐷 có 
giá trị nhỏ, do vậy chúng ta có thể bỏ qua các thành phần tích của chúng với nhau, 
như vậy ta có: 
[0 0 1](�̂� + �̅�) = −[0 0 1]𝐶𝑇(�̂�) 
(�̂�𝐷 + �̅�𝐷 + �̂�𝐷𝑧𝐷 − �̂�𝐷𝑣𝐷 + �̅�𝐷𝑧𝐷 + 2[�̅� ×]�̂�𝐷 + 2[�̅� ×]�̂�𝐷𝑧𝐷) 
(3-16) 
Sử dụng tính chất [𝑎 ×]𝑏 = −[𝑏 ×]𝑎, ta có: 
𝑧𝑑 = [0 0 1](2𝐶
𝑇(�̂�)[(�̂�𝐷 + �̂�𝐷𝑧𝐷) ×]�̅� + 𝑀) (3-17) 
trong đó: 
𝑧𝑑 = [0 0 1](�̂� + 𝐶
𝑇(�̂�)(�̂�𝐷 + �̂�𝐷𝑧𝐷))
𝑀 = 𝐶𝑇(�̂�)�̂�𝐷𝑣𝐷 − �̅� − 𝐶
𝑇(�̂�)�̅�𝐷 − 𝐶
𝑇(�̂�)𝑧𝐷�̅�𝐷
Như vậy phương trình cập nhật của bộ lọc Kalman sử dụng tín hiệu đo của 
cảm biến khoảng cách như sau: 
 Trang 72 
𝑧𝑑 = 𝐻𝑑𝑥 + [0 0 1]𝐶
𝑇(�̂�)�̂�𝐷𝑣𝐷 (3-18) 
trong đó: 
𝐻𝑑 = [0 0 1][2𝐶
𝑇(�̂�)[(�̂�𝐷 + �̂�𝐷𝑧𝐷) ×] −𝐼3 03 −𝐶
𝑇(�̂�) −𝐶𝑇(�̂�)𝑧𝐷] 
[0 0 1]𝐶𝑇(�̂�)�̂�𝐷𝑣𝐷 = 𝑁(0, 𝑅ℎ) đại diện cho nhiễu trắng của việc tính độ cao sử 
dụng tín hiệu khoảng cách từ cảm biến. Hiệp phương sai của nhiễu này được tính như 
sau: 
𝑅ℎ = ([0 0 1]𝐶
𝑇(�̂�)�̂�𝐷)
2𝑅𝐷
2 (3-19) 
Một phương trình cập nhật khác của bộ lọc Kalman được xây dựng dựa trên 
điều kiện vectơ hướng 𝑛𝐷 phải là vectơ đơn vị. Chúng ta có thể triển khai điều kiện 
này như sau: 
‖𝑛𝐷‖ = 1 (3-20) 
Triển khai phương trình trên, ta có: 
‖�̂�𝐷 + �̄�𝐷‖ = 1 (3-21) 
Khai triển bình phương 2 vế phương trình trên: 
�̂�𝐷
𝑇 �̂�𝐷 + 2�̂�𝐷
𝑇 �̄�𝐷 + �̅�𝐷
𝑇 �̄�𝐷 = 1 (3-22) 
Từ điều kiện ‖�̂�𝐷‖ = �̂�𝐷
𝑇 �̂�𝐷 ≈ 1 và �̅�𝐷
𝑇 �̅�𝐷 ≈ 0, có thể xấp xỉ như sau: 
�̂�𝐷
𝑇 �̄�𝐷 = 0 (3-23) 
Như vậy, phương trình cập nhật cho bộ lọc Kalman xây dựng từ vectơ hướng 
của cảm biến khoảng cách như sau: 
0 = 𝐻𝑐𝑥 + 𝑣𝑐 (3-24) 
trong đó: 
- 𝐻𝑐 = [01×12 �̂�𝐷
𝑇 ] 
- 𝑣𝑐 = 𝑁(0, 𝑅𝑐) là nhiễu trắng đại diện cho sai số của việc xấp xỉ trong 
(3-23). Trong đó, độ lớn của vectơ sai số ‖�̅�𝐷‖ ≈ 1%‖𝑛𝐷‖ nên luận án 
chọn 𝑅𝑐 = (10
−2)4𝐼3 = 10
−8𝐼3. 
Tổng hợp các phương trình cập nhật (3-18) và (3-24), ta có phương trình cập 
nhật cho bộ lọc Kalman sử dụng cảm biến khoảng cách như sau: 
 Trang 73 
[
𝑧𝑑
0
] = [
𝐻𝑑
𝐻𝑐
] 𝑥 + [
[0 0 1]𝐶𝑇(�̂�)�̂�𝐷𝑣𝐷
𝑣𝑐
] (3-25) 
Ma trận hiệp phương sai của nhiễu đo như sau: 
𝑅𝑑 = [
𝑅ℎ 01×3
03×1 𝑅𝑐
] (3-26) 
3.5 Thực thi bộ lọc Kalman cho hệ thống 
Quy trình thực thi bộ lọc Kalman cho hệ thống đã được miêu tả một cách chi 
tiết trong Hình 3.2. Trong đó: 
- Tương tự như trong (2-36), việc khởi tạo giá trị ban đầu gồm có khởi tạo 
biến trạng thái ban đầu 𝑥0
− = 015×1 và hiệp phương sai P0
−=015×15. 
- Tại các khoảng thời gian ZVI thì tiến hành cập nhật cho bộ lọc Kalman 
với việc tính các ma trận 𝐻𝑘 và 𝑅𝑘 theo (3-11). Theo miêu tả hệ thống 
thì cứ 3 dữ liệu của IMU thì