PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL) TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG docx

I. LỜI MỞ ĐẦU
Với các phương pháp tính toán truy hồi,
một số công cụ ứng dụng trong đánh giá
chuyển động gọi là Làm trơn (Smoother) đã
được phát triển mạnh mẽ nhằm xác định chính
xác nhất quỹ đạo chuyển động khi đã có tọa
độ điểm xuất phát và kết thúc của hành trình.
Ngược lại với quá trình lọc (Filter) nhằm dự
báo vị trí tiếp theo gần đúng nhất của đối
tượng chuyển động để đưa đối tượng tới đích,
làm trơn (Smoother) là quá trình tính ngược từ
tọa độ đích về tọa độ xuất phát nhằm vẽ ra
quỹ đạo thực, gần như là quỹ đạo trơn nhất
của chuyển động. Kết quả thu được là đường
quỹ đạo nhẵn, gần với thực tế nhất của đối
tượng. Lớp bài toán này thường áp dụng trong
điều khiển hành trình theo quỹ đạo định trước
của vật thể bay (trong quá trình chuyển động
có thể thay đổi quỹ đạo), sử dụng trong đánh
giá quỹ đạo bay của máy bay không người lái.
PHÁT TRIỂN CÔNG CỤ LÀM TRƠN RTS (RAUCH-TUNG-STRIEBEL)
TRONG XÁC ĐỊNH QUỸ ĐẠO CHUYỂN ĐỘNG



ThS. NGÔ THANH BÌNH
Khoa Điện - Điện tử
Trường Đại học Giao thông Vận tải
KS. TRẦN QUỐC TOẢN
Khoa Cơ điện
Trường Đại học Kinh doanh & Công nghệ

Tóm tắt: Để định vị đối tượng chuyển động ta thường sử dụng các công cụ tính toán dự báo
điểm đến tiếp theo của đối tượng trên cơ sở sử dụng bộ lọc Kalman. Tuy nhiên quỹ đạo vẽ được
trên bản đồ không phải là đường quỹ đạo thật mà là một đường zigzag của các điểm dự báo. Trong
đánh giá chuyển động chúng ta cần quỹ đạo thực, vì vậy việc quan tâm tới quá trình làm trơn các
trạng thái đo đạc, gọi là smoother, bao giờ cũng đi kèm theo bài toán định vị chuyển động. Bài báo
này khảo sát và phát triển một số dạng làm trơn quỹ đạo, bao gồm RTS (Rauch-Tung-Striebel
smoother), ETS (extended Forward-Backward smoother). Một số kết kết quả mô phỏng trên
Matlab được đưa ra để minh họa cho các thuật toán này, từ đó tác giả đưa ra một số lưu ý khi khảo
sát quỹ đạo chuyển động.
Summary: To locate moving objects we often use calculator tools to predict the next point of
the objects based on using Kalman filter. But drawing lines on the orbit map are not the actual
trajectory instead of the zigzag lines of predicting points. In the evaluation we need the real orbit,
so the interest in the process as smoothing measurement status, called smoother, always
accompanied by positioning motion problems. This paper surveys and develops some forms of
smoothing orbit, including the RTS (Rauch-Tung-Striebel smoother), ETS (extended Forward-
Backward smoother). A number of the Matlab simulation results are given to illustrate these
algorithm, from that the author gives some notices in surveying trajectory.

II. KHÔNG GIAN TRẠNG THÁI VÀ
LỌC KALMAN
Một mô hình tuyến tính biến đổi theo
thời gian được diễn tả bằng công thức liên tục

về thời gian như sau:

dx(t)
=Fx(t)+Lw(t)
dt
(1)

Trong đó:
Điều kiện quán tính là x(0)~N(m(0),P(0))
F và L ma trận hằng với đặc tính hoạt
động của mô hình
w(t) là ồn trắng với cường độ phổ là Q
c
Bộ lọc Kalman kinh điển được giới thiệu
bởi Rudolph E. Kalman (1960), đưa ra một
giải pháp đệ quy để đánh giá những hệ thống
động rời rạc tuyến tính hóa thời gian. Lọc
Kalman bao gồm 2 bước: bước Dự đoán
(prediction), ở đó các trạng thái tiếp theo của
hệ thống được dự báo bởi các giá trị đo trước
đó; và bước Cập nhật (Update), ở đó các trạng
thái hiện tại của hệ thống được đánh giá bởi
các số liệu đo được tại thời điểm đó. Theo
Särkkä (2006), sử dụng lọc Kalman cho mô
hình trên A
k
và Q
k
có dạng:
(2)
k
A = exp(FΔt)
k
k
⎞
⎟
⎪

⎭
k
-1
k
(3)
k
Δt
T
kKC
0
T
K
Q = exp(F(Δt-τ))LQ L exp
(F(Δt-τ)) dτ
∫
Trong đó: là bước tính.
kk+1
Δt=t -t
(4)
T
C
k
k
T
k
LQ L
C
F0
=exp Δt
D

I
0
-F
⎧⎫
⎞
⎛⎞
⎛⎛
⎪⎪
⎟
⎨⎬
⎜⎟
⎜⎜
⎟
⎝⎠
⎝
⎝⎠
⎪
⎠
⎩
Với :
-1
kk
Q=CD
• Prediction:
(5)
-
kk-1k
m=A m
-T
k k-1 k-1 k-1 k-1

P=A P A +Q


• Update:
-
kkk
v=y-Hm

T
kkk
k
S=HPH+R
k
k
k
k

-T-1
kkk
K=PHS
(6)
-
kkk
m=m+Kv

-T
kkkk
P=P-KSK

Trong đó:

• và là giá trị dự báo trung
bình và ma trận hiệp biến của trạng thái riêng
biệt tại thời điểm k trước khi có giá trị đo đạc
(Tiền nghiệm)
-
k
m
-
k
P
• và là giá trị đánh giá trung
bình và ma trận hiệp biến của trạng thái riêng
biệt tại thời điểm k sau khi có giá trị đo đạc
(Hậu nghiệm)
k
m
k
P
k
v
là giá trị mới đưa vào (thặng dư đo
đạc) tại thời điểm bước k
k
S
là ma trận hiệp biến dự báo đo đạc tại
bước k
k
K
là hệ số khuếch đại lọc, chính là độ
lợi cần tìm của mạch lọc Kalman trong mỗi

ước đoán
Tuy nhiên trong thực tế tồn tại nhiều hệ
thống phi tuyến, ở đó các tính toán của bộ lọc
Kalman truyền thống không được áp dụng
được. Trong những trường hợp này ta sử dụng
mô hình lọc Kalman mở rộng EKF (extended
Kalman filter) như sau:

(7)
kk-1 k
kkk
x = f(x , k -1) + q
y = h(x ,k) + r
⎧
⎨
⎩
-1
)
k
k
k
k
Trong đó:
n
k
xR∈
: là trạng thái của hệ thống ở
bước k
m
k

yR∈
: là giá trị đo đạc tại thời điểm k
k-1 k-1
q~N(0,Q
: là nhiễu xử lý
k
r~N(0,R)
: là nhiễu đo đạc
f: là hàm (phi tuyến) động học của mô
hình
h: là hàm (phi tuyến) vector đo đạc của
mô hình
Bộ lọc Kalman mở rộng và ứng dụng
(Jazwinski, 1970; Maybeck, 1982; Bar-
Shalom, 2001; Grewal và Andrews, 2001;
Särkkä, 2006), mở rộng phạm vi của bộ lọc
Kalman để lọc tối ưu các vấn đề phi tuyến
bằng cách thành lập một xấp xỉ Gaussian phân
phối trạng thái x và các phép đo y dựa trên
biến đổi sử dụng chuỗi Taylor. Sự khác nhau
giữa hai bộ lọc KF và EKF là ma trận và
trong KF được thay thế bởi ma trận
Jacobi và trong
EKF. Giá trị và cũng được tính toán
khác trong EKF, dựa trên biến đổi xấp xỉ
tuyến tính và dạng toàn phương.
k
A
k
H

xk-1
F(m ,k-1)
-
xk
H(m,k)
-
k
m
k
v
• Prediction:

-
kk-1
m = f(m ,k -1)
-T
k x k -1 k -1 x k -1 k -1
P=F(m ,k-1)P F(m ,k-1)+Q

(8)
• Update:

-
kk k
v = y - h(m ,k)
T-
kxkkxk
S=H(m,k)PH(m,k)+R

(9)

-T - -1
kkxk k
K = P H (m ,k)S
-
kkk
m=m+Kv

-T
kkkk
P=P-KSK

Trong đó: và là ma
trận Jacobi của hàm f và h, với
x
F(m,k-1)
x
H(m,k)

j
xj,j'
j'
x=m
δf (x,k -1)
[F (m,k -1)] =
δx
(10)

j
xj,j'
j'

x=m
δh (x,k)
[H (m, k)] =
δx
(11)
III. BỘ LÀM TRƠN SMOOTHER
 Mô hình RTS (Rauch-Tung-
Striebel smoother)
Bộ làm trơn cho mô hình lọc Kalman rời
rạc theo thời gian, còn được biết đến dưới tên
gọi Rauch-Tung-Striebel-smoother RTS
(Rauch, 1965; Gelb, 1974; Bar-Shalom,
2001), sử dụng trong quá trình tính toán làm
trơn mô hình (7) được đưa ra theo phân tán P,
theo công thức:

ss
k1:T k k k
P(x y ) = N(x m ,P )
(12)
Giá trị ước đoán và ma trận hiệp
biến được tính toán trong các dạng làm
trơn cho lọc Kalman, theo công thức sau:
k
m
k
P
-
k+1 k
m=Am

-T
k+1 k k k k
P=APA+Q


T
kkk+1
k
C=PA[P ]
-1
(13)
ss
kkkk+1+
k
m=m+C[m -m ]
1


ss
kk kk+1+1
k
P=P+C[P -P ]

Trong đó:
• và là những ước lượng của
bộ làm trơn (smoother) cho trạng thái ước
đoán và ma trận hiệp biến bước k.
s
k

m
s
k
P
• và là những ước lượng của
bộ lọc (filter) cho trạng thái ước đoán và ma
trận hiệp biến bước k.
k
m
k
P
• và là giá trị ước đoán
trạng thái ước đoán và ma trận hiệp biến bước
k+1
-
k+1
m
-
k+1
P
• là độ lợi làm trơn tại thời điểm
k, chỉ ra với bao nhiêu giá trị làm trơn ước
lượng được chính xác tại bước thời gian cụ
thể.
k
C
Sự khác biệt giữa các bộ lọc Kalman và
RTS là cách tính toán thuận (forward) trong
lọc và tính truy hồi trong bộ làm trơn
(backward smoother). Trong thuật toán làm

trơn RTS tính toán đệ quy bắt đầu từ bước
thời gian cuối cùng T, với và
.
s
T
m=m
T
-1
-
T
k
k
)
s
TT
P=P
 Mô hình ETS (extended Forward-
Backward smoother)
Bộ làm trơn ETS xây dựng các hoạt động
làm trơn như là một sự kết hợp
của hai bộ lọc, trong đó có bộ lọc đầu tiên quét
dữ liệu chuyển tiếp đi từ giá trị đo lường đầu
tiên hướng đến các giá trị mới hơn, và bộ lọc
thứ hai quét ngược theo hướng ngược lại. Kết
quả có thể được hiển thị, kết hợp các ước lượng
được sinh ra bởi hai bộ lọc một cách phù hợp
với một ước tính làm trơn cho trạng thái, trong
đó độ lệch phương sai nhỏ hơn so với bất kỳ lỗi
nào sinh ra trong hai bộ lọc đơn (theo nguyên
tắc Gelb, 1974). Với mô hình forward-

backward, bộ làm trơn ETS có những lỗi tương
tự như các bộ làm trơn RTS, nhưng xử lý được
trong trường hợp phi tuyến. Các giá trị ước
đoán và ma trận hiệp biến được tính
toán trên cơ sở mô hình phi tuyến f như sau:
k
m
k
P
-
k+1 k
m = f(m , k)


-T
k+1 x k k x k k
P=F(m,k)PF(m,k)+Q

(14)
T-
kkxk k+1
C = P F (m ,k)[P ]
ss
kkkk+1k+1
m=m+C[m -m ]

ss-
kk kk+1k-1
P=P+C[P -P ]C

IV. MÔ PHỎNG
Trong bài báo này, tác giả triển khai bộ
lọc Kalman và bộ làm trơn RST, phát triển bộ
làm trơn ETS với xấp xỉ phân tán của trạng
thái nhận được bởi quan sát , trộn
nhiễu Gauss. Khi mô phỏng ta sử dụng hàm
phát sine ngẫu nhiên thay
cho các tín hiệu thực tế nhận được của quỹ
đạo bay thực tế từ tín hiệu GPS và từ sensor
INS, với các tham số hệ số biên độ, vận tốc
góc và độ lớn góc thay đổi, được trộn với
nhiễu ngẫu nhiên. Khi thay thế hàm phát sine
này bằng tín hiệu thu được ngoài thực tế ta sẽ
vẽ được quỹ đạo chuyển động thực của đối
tượng.
k
x
1:k
y
kk
h(x , k) = a sin(θ )
Theo mô hình vận tốc Wiener, vector
trạng thái được biểu diễn như sau:
(15)
(
T
kkkk
x=θωa

dx(t)

dt
= (16)
010
000x(t)+
000
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎝⎠
00
10w(t)
01
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎝⎠
Trong đó:

k
θ
là tham số góc của hàm sin tại thời
điểm k
k
ω
là vận tốc góc trong bước tính thứ k
k
a
là hệ số biên độ tại bước tính thứ k

Từ (2) có:
kkk k-1
1 Δt0
A=exp(FΔt)Þ x = 0 1 0x +q
001
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎝⎠
k-1
k-1
(17)
Với là khoảng cách bước tính, trong
mô phỏng chọn , ngoài thực tế tùy
thuộc tốc độ xử lý của chip ta sẽ tính toán
bước tính này cho phù hợp.
Δt
Δt = 0,01
Từ (3) có:
k
Δt
T
kKC
0
T
K
Q= exp(F(Δt-τ))LQ L exp
(F(Δt-τ)) dτ
∫

Với nhiễu ngẫu nhiên Gauss
ta tính được:
k
q ~ N(0,Q )

32
11
2
k-1 1 1
2
11
Δtq Δtq 0
32
1
Q= Δtq Δtq 0
2
00Δtq
⎛⎞
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎜⎟
⎝⎠
(18)
Với mô hình đo đạc ta có:
(19)

kkkkk
y = h(x ,k) + r = a sin(θ )+r
k
Đạo hàm vector đo đạc, thay vào công
thức mô hình lọc và làm trơn, ta tính được ma
trận Jacobi như sau:

(
)
xkk
H(m,k)=acos(θ )0sin(θ )
k
(20)
Kết quả mô phỏng

Hình 1. Kalman Filter (KF)

Hình 2. Rauch-Tung-Striebel (RTS Smoother)

Hình 3. Extended Forward-Backward
smoother (ETS)
Nhận xét:
Mô hình KF và RST chỉ thực hiện được
khi quỹ đạo phát với sai lệch có biên độ nhỏ
(max ~ 1.5 lần), kết quả thu được gần với quỹ
đạo thực, thời gian tính toán nhanh (t ~ 25 cho
4 chu kỳ).

Với ETS kết quả xử lý sẽ đáp ứng tốt
hơn. Mô phỏng với nhiễu ngẫu nhiên có biên

độ sai lệch lớn (max ~ 4 lần), ta vẫn thu được
quỹ đạo gần đúng với quỹ đạo thực. Tuy
nhiên thời gian tính toán bị kéo dài hơn gấn
gần 10 lần (t ~ 250 cho 4 chu kỳ).
V. KẾT LUẬN
Bộ làm trơn được sử dụng để nâng cao
chất lượng dự đoán và vẽ ra quỹ đạo thực của
chuyển động, bằng cách kết hợp dữ liệu trước
và dữ liệu đã có trong các tính toán nhằm tăng
tần số lấy mẫu, đáp ứng các biên độ sai lệch
lớn, tái tạo quỹ đạo từ dữ liệu thực GPS/INS.
Lớp bài toán này áp dụng trong điều khiển
hành trình theo quỹ đạo định trước của vật thể
bay, thường thấy trong các máy bay không
người lái, đòi hỏi tốc độ tính toán rất cao. Vì
vậy để xác định chính xác được quỹ đạo, ta
cần lưu ý tới một số vấn đề sau:
• Một số hệ thống với các thông số có
thể thay đổi đột ngột có thể không tồn tại ma
trận Jacobi (20). Như vậy quá trình tính toán
có thể rất khó thực hiện được, hoặc tiêu tốn
rất nhiều thời gian để vẽ ra được quỹ đạo.
• Trong một số trường hợp việc tính
toán ma trận Jacobi là rất khó khăn, có thể
không đạt được cả trong tính toán đạo hàm
mô phỏng và lập trình cho chip, kể cả với chip
vi xử lý mạnh. Những lỗi này rất khó để
debug, và rất khó khăn để phát hiện ra với các
đánh giá từng phần rời rạc của hệ thống, đặc
biệt là khi không biết trước quỹ đạo yêu cầu

hoặc quỹ đạo bị thay đổi trong hành trình.

• Sự hội tụ của các thuật toán làm trơn
bị ảnh hưởng rất nhiều bởi quá trình ban đầu
và ma trận hiệp phương sai đo lường, vì vậy
cần lưu ý tới khả năng tồn tại của các ước tính
số liệu ồn trắng trong quá trình xử lý.


Tài liệu tham khảo
[1]. Greg Welch, Gary Bishop (Updated July 24,
2006); An Introduction to the Kalman Filter;
Chapel Hill, NC 27599-3175
[2]. Jim Ledin (2004); Embedded Control
Systems in C/C++: An Introduction for Software
Developers Using MATLAB; CMP Books,
ISBN:1578201276.
[3]. Bruno Otávio Soares Teixeira (2005); Flight
path reconstruction using the unsented Kalman
filter algorithm; Procedings of COBEM 2005, by
ABCM
[4]. Thanh Binh Ngo, Hung Lan Le, Thanh Hai
Nguyen; Survey of Kalman Filters and Their
Application in Signal Processing; Procedings of
AICI 2009, by IEEE and Springer
♦