卡尔曼（Kalman）滤波器的FPGA实现

卡尔曼（Kalman）滤波器的FPGA实现

关于kalman滤波器的原理我这里就不介绍了，直接贴出代码，代码注释中有简要的介绍。该模块适用于一维数据的滤波处理，通过设置Q、R两个参数可以改变其滤波特性。

/******************************FILE HEAD**********************************
 * file_name         : Kalman.v
 * function          : 卡尔曼滤波器
 * author            : 今朝无言
 * date & version    : 2021/12/16 & v1.0
 *************************************************************************/
module Kalman(
input				rst_n,
input		[31:0]	data_in,
input				en,			//数据使能，上升沿有效
output	reg	[31:0]	data_out
);

// kalman滤波
// 流程:
// initial:
//   out=0;
//   P=0;
//   LastP=0;
//   Q=Q0;		//Q、R通过影响卡尔曼增益K=(Pk+Q)/(Pk+Q+R)的值，影响预测值和测量值的权重
//   R=R0;		//Q=inf(过程噪声过大) or R=0(没有观测噪声) 时，完全相信观测值，此时没有滤波效果
// end initial
// while 1:
//   %预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
//   P = LastP + Q;
//   %卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
//   Kg = P / (P + R);
//   %更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
//   out = out + Kg * (input(i) - out);%因为这一次的预测值就是上一次的输出值
//   %更新协方差方程: 本次的系统协方差赋给 kfp->LastP 为下一次运算准备。
//   LastP = (1 - Kg) * P;
//   output(i) = out;
// end while

parameter	Q	= 32'd655;		//参与计算的P,Q,R均为16-16定点数
parameter	R	= 32'd65536;

reg	[31:0]	P		= 32'd0;
reg	[31:0]	LastP	= 32'd0;
reg	[15:0]	Kg		= 16'd0;	//Kg为0-16定点数
reg	[31:0]	LastOut	= 32'd0;

always @(posedge en or negedge rst_n) begin
	if(~rst_n)begin
		P		= 32'd0;
		LastP	= 32'd0;
		Kg		= 16'd0;
		LastOut	= 32'd0;
	end
	else begin
		P			= LastP + Q;	//用‘=’阻塞运算，以保证顺序执行
		Kg			= (P<<16) / (P + R);
		data_out	= (data_in>LastOut)?
					   LastOut + ((Kg * (data_in - LastOut))>>16):
					   LastOut - ((Kg * (LastOut - data_in))>>16);
		LastP		= ((16'd65535 - Kg) * P)>>16;
		LastOut		= data_out;
	end
end

endmodule
//END OF Kalman.v FILE***************************************************

testbench

/******************************FILE HEAD**********************************
 * file_name         : Kalman_tb.v
 * function          : Kalman的testbench
 * author            : 今朝无言
 * date & version    : 2021/12/20 & v1.0
 *************************************************************************/
`default_nettype none
`timescale 1ns/1ps

module Kalman_tb();

reg [15:0]	data_noised	[0:1023];	//带噪数据
reg			start_tb	= 0;

reg				rst_n		= 1;
reg		[31:0]	data_in;
reg				en			= 1;
wire	[31:0]	data_out;

initial begin
	$readmemh("data_noised.hex", data_noised, 0); //读取带噪数据
	#(5); //延迟奇数个ns,以避免读入xxx的data_in
	start_tb 	= 1;
end

initial begin
	wait(start_tb);
	
	#(1024*2);
	#(1024*2);
	
	$stop();
end

always #1 begin
	en	= ~en;
end

reg	[9:0]	k	= 0;
always @(negedge en) begin	//下降沿导入数据，则上升沿可保证向滤波器发送的是当前周期的数据
	if(start_tb)begin
		k		<= k + 1;
		data_in	<= data_noised[k];
	end
end

//Kalman滤波器
Kalman	#(
	.Q(32'd655),
	.R(32'd65536))
Kalman_inst(
	.rst_n		(rst_n),
	.data_in	(data_in),
	.en			(en&start_tb),
	.data_out	(data_out)
);

endmodule
//END OF Kalman_tb.v FILE***************************************************

使用matlab生成噪声数据的.hex文件

%-------------产生一批带噪数据------------------
clc,clear,close all

t=linspace(0,10,1024);
noise=0.2*randn(size(t)); %过程噪声
%data=sin(t)+3*sin(0.2*t+pi/3)+0.3*sin(3*t)+sin(10*t)+noise;
data=(t>5)+noise;

data=data-min(data);
data=data./max(data);
data=floor(data*65535);

plot(t,data)

% fid=fopen('data_noised.hex','w');
% for k=1:1024
%     fprintf(fid,[dec2hex(data(k)),'\n']); %16进制文件
% end
% fclose(fid);

时序仿真