下载文档原格式
UT 变换
核心思想：近似一种概率分布比近似任意一个非线性函数或非线性变换要容易。
假设n 维向量x 经过一个非线性变换得到y ，即()y g x =，x 的均值为ˆx
，协方差矩阵为xx P 。 步骤1：根据x 的均值ˆx
和协方差矩阵xx P ，采用一定的采样策略（此处采用对称采样）得到sigma 点集{}i χ。
0ˆˆˆ1,2,...,i i i n i x
x
x i n χχχ+==+=-=
其中，i 表示矩阵的第i 列。
(0)(0)2()
()/()
/()(1)
1/2(),1,2,...,21/2(),
1,2,...,2m c i m i c W n W n W n i n
W n i n λλλλαβλλ=+=++-+=+==+= 注，这里sigma 点集{}i χ乘以对应的权重{}i m W ，可得sigma 点集的均
值为ˆx
，协方差为xx P 。 步骤2：对所采样的sigma 点集{}i χ中的每个sigma 点通过非线性变
换g(*),得到采样后的sigma 点集{}i y 。
()i i y g χ=
步骤3：对变换后的sigma 点集{}i y 进行加权处理，得到输出变量y
的均值ˆy
和协方差yy P 。 2()02()0ˆˆˆ()()n
i m i
i n i T yy c i i i y W y P W y y
y y ====--∑∑
UKF
非线性系统模型为: ()((1))(1)()(())()
x k f x k V k y k h x k W k =-+-=+ 1) 状态初始条件为 ˆ(0|0)((0|0))ˆˆ(0|0)(((0|0)(0|0))((0|0)(0|0)))T xx x
E x P E x x x x ==--
2) Sigma 点采样
ˆˆ(1|1)[(1|1)(1|1)ˆ(1|1)k k x
k k x k k x k k χ--=----+--
3) 时间更新
202020(|1)((1|1))
ˆ(|1)(|1)
(|1)((|1))
ˆ(|1)(|1)
ˆˆ(|1)(((|1)(|1))((|1)(|1)))(1)n
i m i i n i m i i n
i T xx c i i i k k f k k x k k W k k k k h k k y k k W k k P k k W k k x
k k k k x k k Q k χχχμχμχχ===-=---=--=--=--=------+-∑∑∑
4) 测量更新
20
20
1ˆˆ(|1)((|1)(|1))((|1)(|1))ˆˆ(|1)((|1)(|1))((|1)(|1))()(|1)*(|1)ˆˆˆ(|)(|1)()(()(|1))(|)n i T xy c i i i n i T yy c i i i xy yy xx P k k W k k x
k k k k y k k P k k W k k y
k k k k y k k K k P k k P k k x
k k x k k K k y k y k k P k k χμμμ==--=-------=------=--=-+--∑∑(|1)()(|1)()T xx yy P k k K k P k k K k =---
