无损变换和无迹Kalman滤波算法

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 =---