无迹卡尔曼滤波

非线性系统:

x = f(x, w)         (1)

z = h(x) + v                 (2)

随机信号 w, v分别是过程噪声和观测噪声

CTRV 状态方程

对于const turn rate and velocity magnitude (CTRV )场景:

x = (px, py, v, phi, \dot{phi})

固定速度和转动速率约束,即:

考虑 dv/dt == 0 , dphi^2\dt^2==0\psi是时间的函数, 上述第一项即:

从原状态空间到预测空间,由方程(1),(2)可见,过程噪声w是状态x的非线性项;而z关于观测噪声v是线性的。ukf实际采用增广状态变量sigmax = [x, w]. 过程噪声w包括径向加速度和角加速度 [w_a, w_phi], 且w不是时间的函数 , 对上述第一项可展开:

预测空间

对比扩展卡尔曼 ekf采用一阶线性化近似。无迹卡尔曼ukf,将原状态空间的特征采样点(sigmax)映射到预测空间,采用预测空间里的状态变量f(sigmax)的均值、方差的加权推广作为先验状态估计x^- 和先验误差P^-

其中权值表述:

$$ w = lamda / ( lamda + ns) when i==1 $$ 
$$ w_i = 0.5/(lamba + ns)       when i!=1 $$

$$ X^- =  sum(w_i * f(sigmax) ) $$
$$ P^- =  sum(w_i * (f(sigmax) - x).^2) $$  

观测空间

将原状态空间的特征采样点(sigmax)映射到观测空间,采用观测空间里的状态变量h(sigmax)的均值、方差的加权作为先验观测值Z^- 和观测值先验误差S^-,使用与预测空间同样的权值。

$$ Z^- =  sum(w_i * h(sigmax) ) $$
$$ S^- =  sum(w_i * (Z^- - z).^2) + R $$  

卡尔曼滤波表示: 后验估计(真实状态变量值)与先验估计(预测空间的状态变量值)的差异,可表示为真实观测值与观测空间里的先验观测值的差异的增益 K。

$$ x - x^-  = K (z - Z^-) $$   (3)

可见,卡尔曼增益K在衡量状态误差与观测误差之间的相关性。定义预测空间与观测空间的相关系数:

T =  sum(w_i * (X^- - x)(Z^- - z))
K =  T /  S^-                   (4)

ukf算法

有(4), (3) 分别更新卡尔曼增益和状态变量, 预测空间里的先验误差更新由:

P = P - KSK^t

ps: 在非线性的处理上,线性化或者布点采样都是常用的思路。也是ekf与ukf的区别。

link1

link2