之前已经简单的实现过线性卡尔曼滤波:滤波融合(一)基于C++完成一个简单的 卡尔曼滤波器 线性的系统和测量模型
那么对于非线性的系统,区别就是多了线性化的过程,因为高斯映射到非线性函数,其结果不再是一个高斯分布。根据线性化方法的不同还可以区分为EKF、UKF,目前只介绍EKF,UKF以后有机会的话再说吧~
基础的理论部分看见文章:通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融合
对于线性和非线性的划分,直观的理解就是:只有加法和数乘的就是线性的,除了线性的就都是非线性的。具体可参考:线性和非线性的区别。
那么如何线性化一个非线性函数?扩展的卡尔曼滤波使用的方法叫做一阶泰勒展开式。
h ( x ) ≈ h ( μ ) + ∂ h ( μ ) ∂ x ( x − μ ) h(x)\approx h(\mu)+{∂h(\mu)\over ∂x}(x-\mu) h(x)≈h(μ)+∂x∂h(μ)(x−μ)
还是通过 bfl
库 做对比,使用 bfl
库中提供的例子,相比较线性卡尔曼滤波,系统模型从不带角度信息的,变成带角度信息,使系统方程变成一个非线性函数。
卡尔曼滤波中的更新方程:
X k = X k − 1 + V k − 1 ∗ D e l t a t X_k = X_{k-1}+V_{k-1} *Delta_t Xk=Xk−1+Vk−1∗Deltat
扩展卡尔曼滤波中的更新方程:
X k = X k − 1 + c o s θ ∗ V k − 1 ∗ D e l t a t X_k = X_{k-1} + cos\theta * V_{k-1} * Delta_t Xk=Xk−1+cosθ∗Vk−1∗Deltat
因为考虑了角度的变化,所以变成非线性函数。但是观测模型依然是一个线性模型,和之前一样。
首先看一下在 bfl
中需要做哪些处理,有两个函数需要自己重写,因为这里的系统模型只有自己才知道,以及对应的雅可比矩阵!
//返回条件概率密度的期望值,期望值由系统方程决定
ColumnVector NonLinearAnalyticConditionalGaussianMobile::ExpectedValueGet() const
{
ColumnVector state = ConditionalArgumentGet(0);
ColumnVector vel = ConditionalArgumentGet(1);
state(1) += cos(state(3)) * vel(1);
state(2) += sin(state(3)) * vel(1);
state(3) += vel(2);
return state + AdditiveNoiseMuGet();
}
代码与上面系统方程公式所对应,加上了角度信息,state
就是EKF所维护的状态量,分别包括 x , y , θ x,y,\theta x,y,θ,其中 θ \theta θ 直接累加就可以。然后是系统方程的雅可比矩阵,也就是线性化的关键,因为这里的例子时间间隔都是1,所以 D e l t a t = 1 Delta_t = 1 Deltat=1 直接带入。
x k = x k − 1 + c o s θ ∗ V k − 1 y k = y k − 1 + s i n θ ∗ V k − 1 θ k = θ k − 1 + D e t l a θ x_k = x_{k-1} + cos\theta* V_{k-1} \\ y_k = y_{k-1} + sin\theta* V_{k-1} \\ \theta_k = \theta_{k-1} + Detla_{\theta} xk=xk−1+cosθ∗Vk−1yk=yk−1+sinθ∗Vk−1θk=θk−1+Detlaθ ∂ x k ( μ ) ∂ x = 1 ∂ x k ( μ ) ∂ y = 0 ∂ x k ( μ ) ∂ θ = − s i n θ ∗ V k − 1 {∂x_k(\mu)\over ∂x} = 1 \qquad {∂x_k(\mu)\over ∂y} = 0 \qquad {∂x_k(\mu)\over ∂\theta} = -sin\theta*V_{k-1} ∂x∂xk(μ)=1∂y∂xk(μ)=0∂θ∂xk(μ)=−sinθ∗Vk−1 ∂ y k ( μ ) ∂ x = 0 ∂ y k ( μ ) ∂ y = 1 ∂ y k ( μ ) ∂ θ = c o s θ ∗ V k − 1 {∂y_k(\mu)\over ∂x} = 0 \qquad {∂y_k(\mu)\over ∂y} = 1 \qquad {∂y_k(\mu)\over ∂\theta} = cos\theta*V_{k-1} ∂x∂yk(μ)=0∂y∂yk(μ)=1∂θ∂yk(μ)=cosθ∗Vk−1 ∂ θ k ( μ ) ∂ x = 0 ∂ θ k ( μ ) ∂ y = 0 ∂ θ k ( μ ) ∂ θ = 1 {∂{\theta}_k(\mu)\over ∂x} = 0 \qquad {∂{\theta}_k(\mu)\over ∂y} = 0 \qquad {∂{\theta}_k(\mu)\over ∂\theta} = 1 ∂x∂θk(μ)=0∂y∂θk(μ)=0∂θ∂θk(μ)=1
代码如下所示:
//求第i个条件参数求偏导数。本例中只对状态(第一个条件参数)求偏导数
Matrix NonLinearAnalyticConditionalGaussianMobile::dfGet(unsigned int i) const
{
if (i==0)//derivative to the first conditional argument (x)
{
ColumnVector state = ConditionalArgumentGet(0);
ColumnVector vel = ConditionalArgumentGet(1);
Matrix df(3,3);
df(1,1)=1;
df(1,2)=0;
df(1,3)=-vel(1)*sin(state(3));
df(2,1)=0;
df(2,2)=1;
df(2,3)=vel(1)*cos(state(3));
df(3,1)=0;
df(3,2)=0;
df(3,3)=1;
return df;
}
else ...
}
其他的就和线性卡尔曼滤波一样了,定义完成后在 bfl
库中使用是没有区别的。那么这部分自己该如何实现?
根据一阶泰勒展开式:
h ( x ) ≈ h ( μ ) + ∂ h ( μ ) ∂ x ( x − μ ) h(x)\approx h(\mu)+{∂h(\mu)\over ∂x}(x-\mu) h(x)≈h(μ)+∂x∂h(μ)(x−μ)
h ( x ) = ( x k y k θ k ) = ( x k − 1 + c o s θ ∗ V k − 1 y k − 1 + s i n θ ∗ V k − 1 θ k − 1 + D e t l a θ ) h(x) = \begin{pmatrix} x_k \\ y_k \\ \theta_k \\\end{pmatrix}=\begin{pmatrix} x_{k-1} + cos\theta* V_{k-1} \\ y_{k-1} + sin\theta* V_{k-1} \\ \theta_{k-1} + Detla_{\theta} \\\end{pmatrix} h(x)=⎝⎛xkykθk⎠⎞=⎝⎛xk−1+cosθ∗Vk−1yk−1+sinθ∗Vk−1θk−1+Detlaθ⎠⎞ ≈ ( x 0 + c o s θ ∗ V 0 y 0 + s i n θ ∗ V 0 θ 0 + D e t l a 0 ) + ( 1 0 − s i n θ 0 ∗ V 0 0 1 c o s θ 0 ∗ V 0 0 0 1 ) ( x − x 0 y − y 0 θ − θ 0 ) \approx \begin{pmatrix} x_{0} + cos\theta* V_{0} \\ y_{0} + sin\theta* V_{0} \\ \theta_{0} + Detla_{0} \\\end{pmatrix} + \begin{pmatrix} 1 & 0 & -sin\theta_0 * V_0 \\ 0 & 1 & cos\theta_0 * V_0 \\ 0 & 0 & 1 \\\end{pmatrix}\begin{pmatrix} x-x_0 \\ y-y_0 \\ \theta-\theta_0 \\\end{pmatrix} ≈⎝⎛x0+cosθ∗V0y0+sinθ∗V0θ0+Detla0⎠⎞+⎝⎛100010−sinθ0∗V0cosθ0∗V01⎠⎞⎝⎛x−x0y−y0θ−θ0⎠⎞
此时取 u = 0 u = 0 u=0,在系统模型非线性的情况下,相比较与线性系统,有两点步骤:
- 泰勒在 u = 0 u=0 u=0 处展开,线性化;
- 使用非线性系统的雅可比矩阵 F j F_j Fj ,代替原有的 F F F 矩阵,进行协防差传递。
线性化展开已经做了,并且雅可比矩阵 F j F_j Fj 也已经求出来:
F j = ( 1 0 − s i n θ ∗ V 0 1 c o s θ ∗ V 0 0 1 ) F_j = \begin{pmatrix} 1 & 0 & -sin\theta * V \\ 0 & 1 & cos\theta * V \\ 0 & 0 & 1 \\\end{pmatrix} Fj=⎝⎛100010−sinθ∗Vcosθ∗V1⎠⎞
下面开始修改代码,主要与线性化系统对比说明,线性化系统可以参考滤波融合(一)基于C++完成一个简单的 线性卡尔曼滤波器 进行传感器融合。
首先一些基本的参数维度变成三维了!因为我们的状态量多了 θ \theta θ。
ColumnVector X_k(3);// 状态量 (x,y,θ)
X_k= prior_Mu; // 先验
Matrix P(3,3); // 先验协防差
P = prior_Cov;
Matrix Identity(3,3); // 用于数据格式转换
Identity(1,1) = 1; Identity(1,2) = 0.0; Identity(1,3) = 0.0;
Identity(2,1) = 0.0; Identity(2,2) = 1; Identity(2,3) = 0.0;
Identity(3,1) = 0.0; Identity(3,2) = 0.0; Identity(3,3) = 1;
为了对比系统更新模型,这里把线性系统的代码也拿了过来,可以明显看出来区别,状态量和输入并不是简单的相加,而是相乘,导致了系统模型的非线性。所以对于协防差的传递,需要用到雅可比矩阵。
// 线性系统
//Matrix A(2,2);
//A(1,1) = 1.0; A(1,2) = 0.0; A(2,1) = 0.0; A(2,2) = 1.0;
//Matrix B(2,2);
//B(1,1) = cos(0.8); B(1,2) = 0.0; B(2,1) = sin(0.8); B(2,2) = 0.0;
// X_k = A*X_k + B*input + sysNoise_Mu;
// P = A*(P*A.transpose()) + sysNoise_Cov*Identity;
// 非线性系统
X_k(1) += cos(X_k(3)) * input(1) + sys_noise_Mu(1);
X_k(2) += sin(X_k(3)) * input(1) + sys_noise_Mu(2);
X_k(3) += input(2) + sys_noise_Mu(3);
Matrix df(3,3);
df(1,1)=1; df(1,2)=0; df(1,3)=-input(1)*sin(X_k(3));
df(2,1)=0; df(2,2)=1; df(2,3)=input(1)*cos(X_k(3));
df(3,1)=0; df(3,2)=0; df(3,3)=1;
P = df*(P*df.transpose()) + sys_noise_Cov*Identity;
对于观测值更新模型,该例子中依然是线性的,所以并无差异,这里就不再说明了。对于最终的结果可以对比一下,是没问题的~
P:[2,2]((0.0499,0),(0,0.000204951))# 自己更新的协防差
K:[2,1]((0),(-0.163961))
Z - Hx:[1](0.0327018)
K(Z - Hx):[2](0,-0.00536182)
x + K(Z - Hx):[2](6.86707,7.11033) # 自己更新的状态量
ExpectedValueGet = [2](6.86707,7.11033) measurement = [1](-14.1006) #BFL库更新的状态量
Covariance = [2,2]((0.05,0),(0,0.000204951)) #BFL库更新的协防差
*当预测模型也是非线性的时候,有两点不同:
- y = z − H x y = z -Hx y=z−Hx 变为 y = z − h ( x ) y = z -h(x) y=z−h(x)
- 使用 h ( x ) h(x) h(x)的雅可比矩阵 H j H_j Hj 替换 H H H 矩阵。