![](https://csdnimg.cn/release/download_crawler_static/26486848/bg1.jpg)
状态方程
sin cos
0
cos cos
= 0 cos sin
1 sin tan cos tan
b
ibx x gx
b
iby y gy
b
ibz z gz
n
n
n
g g
y w e
q q
q q q w e
g g q g q w e
æ ö
ç ÷
æ ö
æ ö
æ ö
æ ö æ ö
ç ÷
ç ÷
ç ÷
ç ÷
ç ÷ ç ÷
- - -
ç ÷
ç ÷
ç ÷
ç ÷
ç ÷ ç ÷
ç ÷ ç ÷
ç ÷
ç ÷
ç ÷
ç ÷
è ø è ø
è ø
è ø
è ø
ç ÷
è ø
&
&
&
b
x x ax
n b
y b y ay
b
z z az
v f n
v f n
v f n
æ ö
æ ö
æ ö æ ö
ç ÷
ç ÷
ç ÷ ç ÷
= - +
ç ÷
ç ÷
ç ÷ ç ÷
ç ÷ ç ÷
ç ÷
ç ÷
è ø è ø
è ø
è ø
&
&
&
C g
x ex
y ey
z ez
n
n
n
e
e
e
æ ö æ ö
ç ÷ ç ÷
=
ç ÷ ç ÷
ç ÷ ç ÷
è ø è ø
&
&
&
状态变量 9 维,噪声 9 维
量测方程
x
y
z
v
v
v
y
æ ö
ç ÷
ç ÷
= +
ç ÷
ç ÷
è ø
Z v
量测量 4 维
Simple example
量测
( )
Z sin t v
w
= +
状态
( )
cos
0
t
w w
q
w
æ ö
æ ö
=
ç ÷
ç ÷
è ø
è ø
&
&
如果用
( )
sin t
q w w
= -
&
,
w
符号吧唯一
滤一条有噪声的正弦曲线,确定其频率,效果很好。因为卡尔曼滤波有模型辅助,比一般的
低通滤波器好得多,没有延迟。