Download docx - Chương3 kalman

Transcript
Page 1: Chương3 kalman

Chương 3

THỬ NGHIỆM BỘ LỌC KALMAN CHO MÁY THU GPS

3.1. Mô tả điều kiện thử nghiệm và dữ liệu gốc

Quá trình đo và thu thập dữ liệu được tiến hành trên một đoạn đường đê sông Hồng có chiều dài 5 km (hình 3.1).

Hình 3.1. Quả đường thử nghiệm

Đoạn đường được lựa chọn để thử nghiệm vì:

Đoạn đường có nhiều vị trí cong giúp thử nghiệm khả năng ước lượng của

bộ lọc Kalman với mục tiêu cơ động;

Cao hơn địa hình xung quanh, ít bị ảnh hưởng bởi cây cối, công trình, nhà

cao tầng;

Các vị trí bị che khuất đã được xác định trước.

Page 2: Chương3 kalman

Quá trình đo được thực hiện trong điều kiện thời tiết bình thường, thời gian đo là 10 phút, vận tốc đi chuyển trung bình máy đo là 22km /h. Dữ liệu máy thu được ghi lại theo chuẩn NMEA – chuẩn dữ liệu phổ biến nhất của các máy thu GPS, các thông số được ghi lại tại mỗi điểm đo bao gồm: thời gian, kinh độ, vĩ độ, độ cao, số vệ tinh ...

Hình 3.3. Kết quả đo tại điểm ban đầu

Dữ liệu thu từ máy thu GPS được lưu lại dưới dạng file và đưa lên máy tính để tái hiện và phân tích. Dưới đây là ví dụ các số liệu đo được:

Page 3: Chương3 kalman

Hình 3.2. Bảng kết quả đo tại 1 số điểm

Trong đó các tham số như sau:

- Time: Thời điểm thu;

- Latitude: Vĩ độ điểm khảo sát;

- Longitude: Kinh độ điểm khảo sát;

- E(m): Độ cao so với mực nước biển, đơn vị mét;

- S(km/h): Vận tốc tức thời của máy thu tại điểm khảo sát, đơn vị km/h;

- H(°): Góc tạo bởi phương di chuyển tức thời (tại điểm khảo sát) với phương Bắc, đơn vị độ (°);

- Sat (satelite): Số vệ tinh kết nối được;

- HDOP (Horizon Dilution of Precision): độ mất chính xác theo phương ngang

Page 4: Chương3 kalman

Hình 3.2. Kết quả đo

Dữ liệu đo nhận được là không liên tục, thường xuyên bị gián đoạn do những thời điểm số tín hiệu vệ tinh bắt được không đủ số lượng tối thiểu cần thiết. Trong chương này, chúng ta sẽ sử dụng bộ lọc Kalman trên dữ liệu thu được để tăng độ chính xác và ước lượng đầy đủ quĩ đạo chuyển động của đối tượng.

3.2. Tính toán tham số bộ lọc Kalman cho dữ liệu GPSĐể thiết kế một bộ lọc Kalman ứng dụng cho một đối tượng cụ thể, chúng ta

cần xác định mô hình toán học của mục tiêu và quá trình đo. Chúng ta có thể đưa ra các thong số của bộ lọc như sau:

- Coi quá trình đo tọa độ GPS là quá trình đo 2 giá trị (kinh độ và vỹ độ), chúng ta sẽ không xem xét các dữ liệu khác như thời gian, độ cao, vận tốc…Đơn vị đo là độ, tuy nhiên để tiện lợi trong quá trình tính toán, ta có thể chuyển về đơn vị đo là mét trong mặt phẳng đề-các, các giá trị sau đây cũng sẽ được tính theo đơn vị mét. Ta có vector trạng thái của đối tượng như sau:

Page 5: Chương3 kalman

X=[ z xz yvxv y ]- 2 giá trị được đo là zx và zy, còn giá trị vx và vy là các giá trị thứ cấp,

như vậy ta có ma trận quan sát như sau:

H=[1 0 0 00 1 0 0]

- Mô hình cơ động của mục tiêu được xác định theo mô hình di chuyển của các phương tiện giao thông, có gia tốc tối đa 0.4G(~4m/s2). Sau mỗi chu kỳ đo là 1 giây, sai số do sự cơ động của đối tượng được thể hiện như sau:

Q=[0 0 0 00 0 0 00 0 16 00 0 0 16]

- Mô hình chuyển động của đối tượng là chuyển động đều với vận tốc gần như không đổi nhưng hướng di chuyển có thể thay đổi. Chúng ta coi độ dài vector vận tốc gần như không đổi, còn góc hướng di chuyển (Θ) thay đổi tuyến tính. Để ước lượng quĩ đạo đối tượng ta dùng ma trận chuyển đổi Jacobi như sau:

A=[0 0 cos (ΔΘ) sin (ΔΘ)0 0 −sin (ΔΘ) cos (ΔΘ)0 0 cos (ΔΘ) sin (ΔΘ)0 0 −sin (ΔΘ) cos (ΔΘ)]

Trong đó ΔΘ = Θk - Θk-1; là độ lệch về hướng của vector vận tốc so với lần đo trước đó. Giá trị ước lượng sau mỗi lần đo sẽ được cập nhật theo công thức:

Ẋ = AX

- Hai giá trị đo là zx và zy là độc lập, không phụ thuộc lẫn nhau giá trị hiệp phương sai của chúng là 0. Sai số độ lệch chuẩn của thiết bị thu GPS là khoảng 3m, phương sai của từng giá trị đo độc lập là như vậy ma trận hiệp phương sai có dạng như sau:

Page 6: Chương3 kalman

R=[9 00 9]