二维卡尔曼滤波位置，速度融合

二维卡尔曼滤波位置速度融合

内容简介

1. 小车速度位置建模
2. 基本卡尔曼滤波
3. 最优加权数据融合

小车的位置，速度建模

$$x_t=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]x_{x-1}+\left[\begin{array}{c}\frac{1}{2}\Delta t^2\\ \Delta t\end{array}\right]a_{t-1}$$
这个公式就是根据速度公式来的，是非常简单的速度，加速度公式。 其中$x_t$ 是位置和速度的矩阵。$x_t=\left[\begin{array}{c}p\\ v\end{array}\right]$。其中速度和位置可以根据传感器来获得，速度可以利用车轮的速度传感器，可以对加速度计的测量尽量积分也可以获得。 位置就可以用GPS来获得。而在本文中，我们假定小车是只走直线忽略GPS的横向噪声，来评价卡尔曼滤波的效果。而在加速度计中也忽略x轴，z轴的加速度只考虑y轴的加速度，然后再对其进行积分。 公式中的加速度$a_{t-1}$这里面加速度也可以看成是输入，也就是理解为人踩加速度踏板。

现在我们对这个状态进行观测，但是我们的模型也是会有误差的，对此我们加一个过程噪声，所以这个公式就变成了；
$${\text{x}}_k={\text{F}}_k{\text{x}}_{k-1}+{\text{B}}_k{\text{u}}_k+{\text{w}}_k$$
对应原公式，$w_k$就是过程噪声，并假定其符合均值为零，协方差矩阵为$Q_k$的多元正态分布。
然后，我们对这个小车的状态进行观测。观测方程为；
$${\text{z}}_k={\text{H}}_k{\text{x}}_k+{\text{v}}_k$$
对于$H_k$我的理解就是，你测量到的状态的线性变化方程。比如假定你只能测量到位置，那就是$\left[\begin{array}{cc}0& 1\end{array}\right]$如果能观测到速度，那就是$\left[\begin{array}{cc}1& 0\end{array}\right]$.发散一下，如果两个都能观测到就是一种多传感器的融合。如果你测量到的是超声波声音的返回时间，只要计算成对应的状态即可。$v_k$是观测噪声，其均值为零，协方差矩阵为$R_k$,且服从正态分布。

Kalman Filter卡尔曼滤波

这部分相关的资料太多了，这里就不再赘述了，直接附上维基百科的内容。
卡尔曼滤波是一种递归的估计，即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值，因此不需要记录观测或者估计的历史信息。卡尔曼滤波器与大多数滤波器不同之處，在於它是一种纯粹的时域滤波器，它不需要像低通滤波器等频域滤波器那样，需要在频域设计再转换到时域实现。

卡尔曼滤波器的状态由以下两个变量表示：

${\hat{\text{x}}}_{k|k}{\hat{\text{x}}}_{k|k}$，在时刻k的状态的估计；
${\text{P}}_{k|k}{\text{P}}_{k|k}$，后验估计误差协方差矩阵，度量估计值的精确程度。
卡尔曼滤波器的操作包括两个阶段：预测与更新。在预测阶段，滤波器使用上一状态的估计，做出对当前状态的估计。在更新阶段，滤波器利用对当前状态的观测值优化在预测阶段获得的预测值，以获得一个更精确的新估计值。

预测
$${\hat{\text{x}}}_{k|k-1}={\text{F}}_k{\hat{\text{x}}}_{k-1|k-1}+{\text{B}}_k{\text{u}}_k{\hat{\text{x}}}_{k|k-1}={\text{F}}_k{\hat{\text{x}}}_{k-1|k-1}+{\text{B}}_k{\text{u}}_k$$预测状态方程
$${\text{P}}_{k|k-1}={\text{F}}_k{\text{P}}_{k-1|k-1}{\text{F}}_k^T+{\text{Q}}_k{\text{P}}_{k|k-1}={\text{F}}_k{\text{P}}_{k-1|k-1}{\text{F}}_k^T+{\text{Q}}_k$$预测估计协方差矩阵
Ref：
https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2

最优加权数据融合

个人认为这个是数据融合中最简单的一种了，这个就相当于两个传感器占比多少，不同于简单的占比多少，这个具体到某一时刻的占比为多少$w_i$。该系数对上一个时刻有一个评价并且会影响下一个时刻的占比系数。用数学公式来表达是这样的；
假设有$N$个传感器，
$${\hat{x}}_k=\sum _{i=1}^n{w}_i{x}_{ik}$$
为了保证是无偏估计，$E[{\hat{x}}_k]=x_k$ 也就是说 $w_1+w_2+...+w_n=1$, 我们假设${ϵ}_i$是第i个传感器的误差，这个误差假定为高斯白噪声${ϵ}_i\in N(0,{\sigma }_i^2)$ 各个传感器之间噪声相互独立。${\sigma }_i^2$为传感器观测噪声的方差。最优的占比系数就为该传感器噪声比上总噪声，即；
$$w_i^{\prime }=\frac{1}{{\sigma }_i^2{\sum }_{j=1}^n\frac{1}{{\sigma }_j^2}}$$

实验仿真

该图由假定的观测到的位置求导得到速度与真实值的误差

该图是GNSS数据求导得到的速度曲线

该图是加速度计得到的误差

该图是加速度计的加速度测量曲线

实验初始设定为

在50个采样时间后有一个2米每秒方的加速度持续50个采样时间，这个加速度相当于给速度一个扰动。初始速度为20m/s。所以
$$v=\{\begin{array}{l}20,0<t\le 5\\ 2t,5<t\le 10\\ 30,t>10\end{array}$$
融合后数据为；

这里的融合是加速度融合，利用位置的二次求导得到加速度和加速度计的数据进行融合。

Matlab Code

clear all ;
close all
clc
data = xlsread( ‘MI10_ACC_XY.xlsx’);
a_y = data(:,2) - 0.62;
N = length(a_y);
T = 0.01;
v_y = zeros(N,1);
v_y(:,1) = 0;
for i = 2 : N;
v_y(i,:) = v_y(i-1,:)+a_y(i-1,:)0.01;
end
H = [0 1];
F = [1 T;0 1];
B = [T^2/2 ; T];
x = zeros(2,N);
x(:,1) = [0;0];
xp = zeros(2,N);
xp(:,1) = [0.1;0.1];
v = randn(N,1);
z = v_y + 0.1v;
w = randn(1,N);
P=[1.2 0.9;0.8 1.3];
Q=cov(w);
R=cov(v);
for j=2:N;
xp(:,j) = Fx(:,j-1) + [(T^2/2)a_y(j-1);Ta_y(j-1)] + [0;0.01w(:,j)];

Pp(:,:,j)=F.*P(:,:,j-1).*F'+[0 0;0 0.01*Q];

K=(Pp(:,:,j)*H')/(H*Pp(:,:,j)*H'+R);

x(:,j)=xp(:,j)+K*(z(j)-H*xp(:,j));

P(:,:,j)=(eye(2)-K*H)*Pp(:,:,j);

end

三传感器融合代码示例；
w1=var(longitude_fil-y);
w2=var(longitude2_fil-j);
w3=var(longitude3_fil-f);
disp(‘w1=’);disp(w1);
disp(‘w2=’);disp(w2);
disp(‘w3=’);disp(w3);
result=(w3/(w1+w2+w3))*longitude_fil+(w1/(w1+w2+w3))*longitude2_fil+(w2/(w1+w2+w3))*longitude3_fil;

谢谢观看！