非线性系统:
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的区别。