滤波融合（二）基于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)$

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

$\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 -Hx$ 变为 $y = z -h(x)$
使用 $h(x)$ 的雅可比矩阵 $H_j$ 替换 $H$ 矩阵。

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

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