基于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身上了，希望早日摆脱雾霾，早日活力满满！