无迹卡尔曼滤波

非线性系统：

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})

固定速度和转动速率约束，即：

$\dot{x} = (\dot{p_x}, \dot{p_y}, \dot{v}, \dot{\psi}, \ddot{\psi}) = (vcos(\psi ), vsin(\psi), 0, \dot{\psi}, 0)$

$x_{k+1} = x_k + \int_{t_k}^{t_{k+1}} \dot{x} dt + w$

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

$px_{k+1} = px_k + \int_{t_k}^{t_{k+1}} v cos\psi) dt + w \\ = px_k + {\frac{sin\psi}{\dot{\psi}}}_{t_k}^{t_{k+1}}$

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

$px_{k+1} = px_k + \int_{t_k}^{t_{k+1}} \dot{px} dt + \int_{0}^{\Delta t} w_a \Delta t \cos{\psi}dt = px_k + \frac{sin{\psi}}{\dot{\psi}}_{t_{k+1}}^{t_k} + 0.5 w_a cos{\psi} \Delta t^2$

预测空间

对比扩展卡尔曼 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