7. Bố cục chung của luận á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 tố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
rD b
nD b
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 tố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 tốn INA ở Hình 2.12.
Ngồ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à tồ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 = [ 𝐴12 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)
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 đó 𝑟 = [𝑟𝑥 𝑟𝑦 𝑟𝑧]𝑇.
Trang 70 𝑣̅ = −𝑣̂
𝑟̅𝑧 = −𝑟̂𝑧 (3-10)
Vậy phương trình cập nhật cho bộ lọc Kalman như sau:
𝑧𝑍𝑉𝐼 = 𝐻𝑍𝑉𝐼𝑥 + 𝑣𝑍𝑉𝐼 (3-11) trong đó: - 𝑧𝑍𝑉𝐼 = [𝑟̂𝑣̂ 𝑧 ] - 𝐻𝑍𝑉𝐼 = [003×6 𝐼3 03×6 1×5 1 03×3 03×6] - 𝑣𝑍𝑉𝐼 = [𝑣𝑣𝑣 𝑟𝑧] - 𝑅𝑍𝑉𝐼 = [0𝑅𝑣 03×1 1×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 hồn tồ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 vng góc với mặt đất trong q 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: 𝑅𝑑 = [0𝑅ℎ 01×3
3×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ì có một dữ liệu của cảm biến khoảng cách. Tại các thời điểm có tín hiệu của cảm biến khoảng cách nhưng không thuộc khoảng thời gian ZVI thì tiến hành cập nhật cho bộ lọc Kalman