|
之前已经简单的实现过线性卡尔曼滤波：滤波融合（一）基于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?1?yk?=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
代码如下所示：
Matrix NonLinearAnalyticConditionalGaussianMobile::dfGet(unsigned int i) const
{
if (i==0)
{
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)=???xk?yk?θk?????=???xk?1?+cosθ?Vk?1?yk?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θ?V0?y0?+sinθ?V0?θ0?+Detla0?????+???100?010??sinθ0??V0?cosθ0??V0?1???????x?x0?y?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?=???100?010??sinθ?Vcosθ?V1????
下面开始修改代码，主要与线性化系统对比说明，线性化系统可以参考滤波融合（一）基于C++完成一个简单的 线性卡尔曼滤波器 进行传感器融合。
首先一些基本的参数维度变成三维了！因为我们的状态量多了
θ
\theta
θ。
ColumnVector X_k(3);
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;
为了对比系统更新模型，这里把线性系统的代码也拿了过来，可以明显看出来区别，状态量和输入并不是简单的相加，而是相乘，导致了系统模型的非线性。所以对于协防差的传递，需要用到雅可比矩阵。
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)
Covariance = [2,2]((0.05,0),(0,0.000204951))
*当预测模型也是非线性的时候，有两点不同：
-
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 矩阵。
|