滤波融合（二）基于C++完成一个简单的扩展卡尔曼滤波器的非线性系统模型

之前已经简单的实现过线性卡尔曼滤波：滤波融合（一）基于C++完成一个简单的卡尔曼滤波器线性的系统和测量模型
那么对于非线性的系统，区别就是多了线性化的过程，因为高斯映射到非线性函数，其结果不再是一个高斯分布。根据线性化方法的不同还可以区分为EKF、UKF，目前只介绍EKF，UKF以后有机会的话再说吧～
基础的理论部分看见文章：通俗易懂理解扩展卡尔曼滤波EKF用于多源传感器融合
对于线性和非线性的划分，直观的理解就是：只有加法和数乘的就是线性的，除了线性的就都是非线性的。具体可参考：线性和非线性的区别。

那么如何线性化一个非线性函数？扩展的卡尔曼滤波使用的方法叫做一阶泰勒展开式。
$h(x)\approx h(\mu)+{∂h(\mu)\over ∂x}(x-\mu)$

还是通过 bfl 库做对比，使用 bfl 库中提供的例子，相比较线性卡尔曼滤波，系统模型从不带角度信息的，变成带角度信息，使系统方程变成一个非线性函数。
卡尔曼滤波中的更新方程：
$X_k = X_{k-1}+V_{k-1} *Delta_t$
扩展卡尔曼滤波中的更新方程：
$X_k = X_{k-1} + cos\theta * V_{k-1} * Delta_t$
因为考虑了角度的变化，所以变成非线性函数。但是观测模型依然是一个线性模型，和之前一样。

首先看一下在 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，\theta$ ，其中 $\theta$ 直接累加就可以。然后是系统方程的雅可比矩阵，也就是线性化的关键，因为这里的例子时间间隔都是1，所以 $Delta_t = 1$ 直接带入。
$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}$ ${∂x_k(\mu)\over ∂x} = 1 \qquad {∂x_k(\mu)\over ∂y} = 0 \qquad {∂x_k(\mu)\over ∂\theta} = -sin\theta*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}$ ${∂{\theta}_k(\mu)\over ∂x} = 0 \qquad {∂{\theta}_k(\mu)\over ∂y} = 0 \qquad {∂{\theta}_k(\mu)\over ∂\theta} = 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)\approx h(\mu)+{∂h(\mu)\over ∂x}(x-\mu)$
$\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}$ $\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}$

此时取 $u = 0$ ，在系统模型非线性的情况下，相比较与线性系统，有两点步骤：

泰勒在 $u = 0$ 处展开，线性化;
使用非线性系统的雅可比矩阵 $F_j$ ，代替原有的 $F$ 矩阵，进行协防差传递。

线性化展开已经做了，并且雅可比矩阵 $F_j$ 也已经求出来：
$F_j = \begin{pmatrix} 1 & 0 & -sin\theta * V \\ 0 & 1 & cos\theta * V \\ 0 & 0 & 1 \\\end{pmatrix}$

下面开始修改代码，主要与线性化系统对比说明，线性化系统可以参考滤波融合（一）基于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 - h (x)$
使用 $h (x)$ 的雅可比矩阵 $H_j$ 替换 $H$ 矩阵。

滤波融合（二）基于C++完成一个简单的 扩展卡尔曼滤波器的非线性系统模型

猜你喜欢

滤波融合（二）基于C++完成一个简单的扩展卡尔曼滤波器的非线性系统模型