基于FPGA设计的kalman滤波器——QMJ

基于Verilog设计的kalman滤波器

   本设计将通过matlab和verilog分别编写的kalman程序，对同一组数据进行处理，将数据处理效果对比，分析verilog的kalman滤波效果。

一、仿真软件

本文kalman滤波仿真使用的软件包括：Quartus II，ModelSim，Matlab。
Quartus II仿真设备选择5CSEMA5F31C6。

二、Kalman滤波介绍

1，Kalman滤波用途：

   用于机器人导航，控制，传感器数据融合以及军事方面的雷达系统、导弹追踪等等。近年来多应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2，Kalman滤波作用：

   卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。
   在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测。

3，Kalman公式:

1） 状态方程

状态方程由检测的状态变量组成。
式中各参数代表含义：
ut: 系统控制输入
wt: 当前时刻的过程噪声
A: 状态转移矩阵
B: 控制矩阵

2）观测方程

由状态方程得到输入信号Z。
式中各参数代表含义：
H: 观测转移矩阵，表示状态值和观测值之间的转换关系
Vt: 当前时刻的观测噪声

3）预测方程

式中各参数代表含义：
X(k|k）: 当前时刻状态矩阵预测值
X(k|k-1) : 上一时刻状态矩阵
GQG: E(wt wtT)
P(k|k-1): 上一时刻协方差矩阵
P(k|k): 当前时刻协方差矩阵预测值

4）更新方程:

式中各参数代表含义：
X(k+1|k): 当前时刻状态矩阵修正值
P(k+1|k): 当前时刻协方差矩阵修正值
K(k): kalman增益
R : E(vt vtT)
I : 单位矩阵

三、 程序框架

注：保存输入数据是由于每个42个时钟才能处理一个输入数据；
X的初始值为零；
P的初始值等于E(wt wtT)

   第1步，输入信号是指拉压力传感器采集的离散化数据。

   第2步，通过matlab程序进行这一步处理。将采集的数据进行归一化处理，并转换成Q15格式的二进制数据。，这样处理数据的原因，是由于FPGA硬件容易对二进制数据进行运算，并且在用除法器计算kalman增益过程中，所用的数据都是Q15格式。

   第3步，由kalman中预测方程和更新方程编写的verilog程序对数据进行处理。
   （1）通过状态方程和观测方程得到状态矩阵X和协方差矩阵P的初始值。
   （2）输入数据Z、状态矩阵X和协方差矩阵P的寄存器，每隔42个时钟向预测方程和更新方程输入数据。
   （3）通过预测方程和更新方程对从寄存器中得到的Z、上一时刻的X和P进行处理，得到当前时刻的状态矩阵X和协方差矩阵P的修正值。
   （4）将数据处理过程得到的X、P的修正值传递到X、P的寄存器中。

   第4步，将从状态矩阵选取所需要的状态变量的数值进行保留。通过matlab程序将保留的数值转换成十进制。

   第5步，得到输入信号过滤后的数据，可通过matlab程序对前后数据波形进行对比。

四、 仿真波形

1，ModelSim仿真时序如下。
   每42个时钟处理一个数据。若采用FPGA硬件提供50MHZ时钟周期，则大约0.84微秒处理一个16比特的数据。（图3中z_in代表的时序，是待数据的输入；x_est1代表的时序，表示处理后的数据）

图3 ModelSim仿真时序

2，信号数据分别通过Matlab和verilog编写的kalman程序，处理后信号波形对比如下。
   下面所展示的图形，都是将程序处理后的数据放在matlab中绘制成图像。
   图1是拉压力传感器采集的数据经过归一化处理后的，图2是通过matlab设计的程序处理后的信号，图3是通过verilog设计的程序处理后的信号。
   由图2和图3仿真波形对比，可以看出本设计verilog的kalman滤波，将会在FPGA中实现良好的滤波效果。

图6 FPGA处理后的数据

五、Verilog的kalman滤波现所到达的硬件指标：

1）端口32个
2）寄存器663个
Verilog程序综合后生成的报告如下图：