基于Hdl Coder实现卡尔曼滤波算法

    总所周知,FPGA极其不擅长复杂算法的运算,但是如果项目中又涉及一些高级算法的实现,在没有可封装IP核调用的形式下,我们应该如何进行程序开发呢?今夕已经是2020年,我们一味依赖于用verilog写代码无异于用汇编写程序,这种方式无异于古时钻木取火的原始时代。如今用Matlab联调FPGA,基于simulink的Hdl Coder模块搭建算法模型,再自动生成代码才是高阶有效的终极玩法。尤其在一些信号处理领域,掌握Matlab联调FPGA的技术更是必备技能。

 下面以卡尔曼滤波为例,具体讲述如何基于Hdl Coder实现卡尔曼滤波算法实现。首先简单介绍下卡尔曼滤波算法:

卡尔曼滤波(Kalman filtering)是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。由于观测数据中包含系统噪声和干扰的影响,所以最优估计也可看作是滤波过程。简单来说,它就是利用过去的状态值和现在的测量值来更正现在的状态值,利用卡尔曼增益不停在估计和测量中寻找最优化的平衡值。

Kalman filtering的经典五方程,在进行卡尔曼滤波的程序设计前必须要充分理解这五个方程的定义、推导以及相关变量的设定。

 (1) 对于现在状态的预测方程:

X(k|k-1)=A X(k-1|k-1)+B U(k) ……….. (1)

P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)

(2)对于现在状态的更新方程:(拿过去的真实值得到现在的预测值,再集合现在的测量值Z(k),进行现在状态值的更新)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)

P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)

其中Z(k)=H X(k)+V(k)为观测方程。

对于上述方程要明确以下几个变量的赋值:

1) 状态转移矩阵:A

2) 系统控制变量:U(k)

3) 观测矩阵:H

4)系统状态初值:X(0|0)

5)系统协方差:P(0|0)

6)R:测量噪声

7)Q:过程噪声

8)Z:观测值

这7个变量要根据不同的产品以及应用场景进行具体赋值,其中对于单系统输入,变量赋值为:

1)A=1(状态转移矩阵),U(k)=0(系统控制变量), H=1(观测矩阵),B=1(对于一维变量,全为1)

2)Z观测值就是系统外部灌进来的实时变量。

3)系统协方差:P(0|0),可以采样一段时间后进行运算保证在kalman滤波器工作前进行赋值即可。

4)对于卡尔曼应用,最难确定的就是Q、R这两个噪声,只能根据实际模型,不停调整以逼近最优解。

基于上述的卡尔曼滤波搭建simulink仿真模型

 

 

 

 

 下面对上述simulink模型进行进一步封装

 

 

 具体设置kalman模块的Hdl Coder参数模式

 

 

 

 

 

 

 

 最后点击“Generate HDL”与“Generate Test Bench”,生成verilog源程序与测试代码

 

 

 

要对执行文件进行仿真,只需要在modelsim命令框自动执行以下后缀文件即可“kalman_compile.do”,”kalman_tb_compile.do”,”kalman_tb_sim.do”即可。

下面进行卡拉曼滤波效果展示:

验证目的:调整Q的参数值,验证下Modelsim的仿真波形是否有对应的正确变化
1)Q=0.003,R=0.0001,,预计结果滤波后的噪声跟原始噪声相差不大。

 

 

 

 

 

 

结果展示:

 

 

 

验证结果:原始噪声与滤波后噪声幅值基本一致,符合期望值。

2)Q=0.0000003,R=0.0001,预计滤波后的噪声跟原始噪声幅度值相差较大,滤波效果较好。

 

 

结果展示:

 

验证结果:卡尔曼滤波后噪声幅值较小,符合输出期望值。
 
因此,基于Hdl Coder实现卡尔曼滤波算法结束。
 
Ps:最近一周心情太差,压力满满,茶饭不思,唯有将无限的精力投身于自己喜欢的FPGA身上了,希望早日摆脱雾霾,早日活力满满!