Python与Verilog实现卡尔曼滤波器 —— 机器人位置估计

为了更好阅读体验建议点击阅读原文阅读

一、引言：为什么机器人需要卡尔曼滤波？

想象你开发了一个可以在树林里自主导航的机器人。为了知道它的实时位置，你在机器人上安装了GPS传感器，但GPS的精度大约为10米——在布满沟壑和悬崖的树林里，10米的误差足以让机器人坠入深渊。

此时，你还可以获取一些额外的运动信息：里程计可以记录机器人的行走距离，惯性测量单元（IMU）可以感知姿态变化。但这些传感器同样存在噪声和漂移。GPS告诉了你"大概在哪"，里程计告诉了你"走了多远"，但两者都不完美。

卡尔曼滤波（Kalman Filter） 的核心思想正是：综合利用所有可用的信息，根据其本身的噪声特性分配权重，得到一个比任何单一估计都更准确的结果。

本文将从原理出发，分别用 Python（浮点运算，算法验证） 和 Verilog（定点运算，硬件加速） 实现一维卡尔曼滤波器，用于机器人位置估计。

二、卡尔曼滤波的五大核心公式

卡尔曼滤波是一种最优线性递归估计算法，由预测（Prediction）和更新（Update）两个阶段组成，共五个核心公式：

对于线性离散系统，卡尔曼滤波通过以下五个公式递归运行：

三、以机器人为例卡尔曼公式实现和说明

下面我们用一个例子来说明上述卡尔曼滤波公式的内在逻辑。

假如我们有一台轮式机器人 Robo 如图所示，它只能够进行一维直线运动。

设计运动

为了更好阅读体验建议点击阅读原文阅读

import numpy as np

import math

import matplotlib.pyplot as plt

if __name__=="__main__":

## 1.设计一个匀加速直线运动，以观测此运动

X_real = np.mat(np.zeros((2, 100))) # 空矩阵，用于存放真实状态向量

X_real[:, 0] = np.mat([[0.0], # 初始状态向量

  [1.0]])

a_real = 0.1# 真实加速度

F = np.mat([[1.0, 1.0], # 状态转移矩阵

[0.0, 1.0]])

Q = np.mat([[0.0001, 0.0], # 状态转移协方差矩阵，我们假设外部干扰很小

[0.0, 0.0001]])

B = np.mat([[0.5], # 控制矩阵

[1.0]])

for i in range(99):

X_real[:, i + 1] = F * X_real[:, i] + B * a_real # 计算真实状态向量

X_real = np.array(X_real)

fig = plt.figure(1)

plt.grid()

plt.title('real displacement')

plt.xlabel('k (s)')

plt.ylabel('x (m)')

plt.plot(X_real[0, :])

plt.show()

fig = plt.figure(2)

plt.grid()

plt.title('real velocity')

plt.xlabel('k (s)')

plt.ylabel('v (m/s)')

plt.plot(X_real[1, :])

plt.show()

X_real = np.mat(X_real)

传感器的观测值

为了更好阅读体验建议点击阅读原文阅读

## 2.建立传感器观测值

z_t = np.mat(np.zeros((2, 100))) # 空矩阵，用于存放传感器观测值

H = np.mat(np.zeros((2, 2)))

H[0, 0], H[1, 1] = -1.0, -1.0

noise = np.mat(np.random.randn(2,100)) # 加入位移方差为1，速度方差为1的传感器噪声

R = np.mat([[1.0, 0.0], # 观测噪声的协方差矩阵

[0.0, 1.0]])

for i in range(100):

z_t[:, i] = H * X_real[:, i] + noise[:, i]

z_t = np.array(z_t)

fig = plt.figure(3)

plt.grid()

plt.title('sensor displacement')

plt.xlabel('k (s)')

plt.ylabel('x (m)')

plt.plot(z_t[0, :])

plt.show()

fig = plt.figure(4)

plt.grid()

plt.title('sensor velocity')

plt.xlabel('k (s)')

plt.ylabel('v (m/s)')

plt.plot(z_t[1, :])

plt.show()

z_t = np.mat(z_t)

卡尔曼滤波

前两步的操作主要是为了获取数据，现在我们利用预测和更新公式对以上数据进行卡尔曼滤波：

## 3.执行线性卡尔曼滤波

Q = np.mat([[1.0, 0.0], # 状态转移协方差矩阵，我们假设外部干扰很小，

[0.0, 1.0]])# 转移矩阵可信度很高

# 建立一系列空序列用于储存结果

X_update = np.mat(np.zeros((2, 100)))

P_update = np.zeros((100, 2, 2))

X_predict = np.mat(np.zeros((2, 100)))

P_predict = np.zeros((100, 2, 2))

P_update[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值

[0.0, 1.0]])

P_predict[0, :, :] = np.mat([[1.0, 0.0], # 状态向量协方差矩阵初值

[0.0, 1.0]])

for i in range(99):

# 预测

X_predict[:, i + 1] = F * X_update[:, i] + B * a_real

P_p = F * np.mat(P_update[i, :, :]) * F.T + Q

P_predict[i + 1, :, :] = P_p

# 更新

K = P_p * H.T * np.linalg.inv(H * P_p * H.T + R) # 卡尔曼增益

P_u = P_p - K * H * P_p

P_update[i + 1, :, :] = P_u

X_update[:, i + 1] = X_predict[:, i + 1] + K * (z_t[:, i + 1] - H * X_predict[:, i + 1])

X_update = np.array(X_update)   

X_real = np.array(X_real) 

fig = plt.figure(5)

plt.grid()

plt.title('Kalman predict displacement')

plt.xlabel('k (s)')

plt.ylabel('x (m)')

plt.plot(X_real[0, :], label='real', color='b')

plt.plot(X_update[0, :], label='predict', color='r')

plt.legend()

plt.show()

fig = plt.figure(6)

plt.grid()

plt.title('Kalman predict velocity')

plt.xlabel('k (s)')

plt.ylabel('v (m/s)')

plt.plot(X_real[1, :], label='real', color='b')

plt.plot(X_update[1, :], label='predict', color='r')

plt.legend()

plt.show()

X_update = np.mat(X_update)

X_real = np.mat(X_real)

四、Verilog实现：定点运算与硬件加速

当我们需要将卡尔曼滤波部署到FPGA或ASIC上时，Python的浮点运算就不再适用。硬件实现需要考虑：定点数表示、流水线设计、时序约束和资源复用。附件中的Verilog代码展示了一个轻量级、10级流水线的卡尔曼滤波器实现。

4.1 系统架构

整个系统分为5个模块：

以及3个共享运算器：add（加减法）、mux（乘法）、div（除法）。

4.2 核心代码解析

（1）Kalman_1：状态预测（公式1）

module Kalman_1(

input   clk_50M,

input   Rst_n,

input [15:0] X_last, // t-1时刻的后验估计

input [15:0] B_X_last,   // 控制输入（预留，当前为0）

output reg [15:0]   X_  // t时刻的先验估计

);

// 流水线计数 0~9

always@(posedge clk_50M or negedge Rst_n) begin

if(!Rst_n)

X_ <= 16'd0;

else begin

case (Count)

4'd1: begin

X_ <= X_last; // X^- = X_last（简化模型）

end

// ... 其他时钟周期可用于更复杂的预测

endcase

end

end

endmodule

设计要点：当前实现为简化模型，直接令 X^- = X_last。B_X_last 预留了控制输入接口，可扩展为带速度项的运动模型。

（2）Kalman_2_3：协方差预测与增益计算（公式2、3）

module Kalman_2_3(

input   clk_50M,

input   Rst_n,

input [15:0] P_last,

output reg [15:0]   P_,

output reg [15:0]   Kg

);

reg [15:0] Q_error = 0; // 系统过程协方差

reg [15:0] R_error = 3; // 测量噪声协方差

always@(posedge clk_50M or negedge Rst_n) begin

case (Count)

4'd1: begin

ADD_sub   <= 1'd1;  // 加法

ADD_dataa <= P_last * 'd100; // P_last 放大100倍

ADD_datab <= Q_error;   // + Q

end

4'd2: begin

P_ <= ADD_result; // P^-（仍放大100倍）

ADD_dataa <= ADD_result / 'd10; // 缩小10倍

ADD_datab <= R_error * 'd10; // R 放大10倍

end

4'd3: begin

DIV_dataa <= P_; // 被除数 = P^-（×100）

DIV_datab <= ADD_result; // 除数 = P^-/10 + R×10

end

4'd4: begin

Kg <= DIV_result;   // Kg = P^- / (P^- + R)

P_ <= P_ / 'd100;   // P^- 恢复原始量级

end

endcase

end

endmodule

定点数与缩放处理：这是硬件实现中最关键的部分。由于16位定点数无法直接表示小数，代码采用了缩放因子（Scaling Factor）策略：

• P_last * 100
  ：将协方差放大100倍，避免小数被截断
• R_error * 10
  ：测量噪声放大10倍
• 最终 Kg 的计算结果也需要在后续模块中相应缩放

// 在Kalman_4_5中处理缩放还原

MULT_datab <= ADD_result / 'd10; // 乘法结果缩小10倍

ADD_datab  <= Kg / 10;  // Kg缩小10倍用于1-Kg计算

（3）Kalman_4_5：状态与协方差更新（公式4、5）

module Kalman_4_5(

input   clk_50M,

input   Rst_n,

input [15:0] in_data, // 测量值 Z

input [15:0] X_, // 先验估计

input [15:0] P_, // 先验协方差

input [15:0] Kg, // 卡尔曼增益

output reg [15:0]   X,  // 后验估计

output reg [15:0]   P   // 后验协方差

);

always@(posedge clk_50M or negedge Rst_n) begin

case (Count)

4'd2: begin

// 计算 |Z - X^-|，通过比较大小确定减法顺序

if(in_data > X_) begin

ADD_dataa <= in_data;

ADD_datab <= X_;

end else begin

ADD_dataa <= X_;

ADD_datab <= in_data;

end

end

4'd5: begin

MULT_dataa <= Kg;

MULT_datab <= ADD_result;   // Kg * |Z - X^-|

end

4'd6: begin

// 根据in_data和X_的大小关系，决定加或减

if(in_data > X_)

ADD_sub <= 1'd1; // 加法：X = X^- + Kg*(Z-X^-)

else

ADD_sub <= 1'd0; // 减法：X = X^- - Kg*(X^- - Z)

ADD_dataa <= X_;

ADD_datab <= MULT_result / 'd10;

end

4'd7: begin

X <= ADD_result; // 输出更新后的状态

ADD_dataa <= 16'd1;

ADD_datab <= Kg / 10;   // 1 - Kg/10

end

4'd8: begin

MULT_dataa <= P_;

MULT_datab <= ADD_result;   // P = P^- * (1 - Kg)

end

4'd9: begin

P <= MULT_result;   // 输出更新后的协方差

end

endcase

end

endmodule

有符号数处理技巧：由于Verilog中 [15:0] 默认是无符号数，代码巧妙地通过先取绝对值做减法，再根据大小关系决定加减，避免了有符号数运算的复杂性。

（4）Kalman_ctrl：数据源控制

该模块模拟了传感器数据源。实际应用中，这里可以替换为ADC接口、串口接收或传感器总线（如I2C/SPI）。

4.3 10级流水线时序

每个滤波周期 = 10个时钟周期 @ 50MHz = 200ns，即每秒可处理500万次滤波迭代，这是硬件加速相比软件的巨大优势。

五、Python vs Verilog：对比分析

关键差异——定点数缩放：

Python中可以直接写 Kg = P_pred / (P_pred + R)，而Verilog中必须手动管理数值范围：

# Python：自然优雅

Kg = P_pred / (P_pred + R)   # 0.09...

X = X_pred + Kg * (z - X_pred)

// Verilog：需要精心设计的缩放

// P_last * 100 -> ADD -> /100 -> DIV -> Kg

// Kg / 10 -> MULT -> /10 -> ADD -> X

这种"不优雅"是硬件实现的代价，但也是达到实时性能的必要手段。

六、总结

卡尔曼滤波是机器人状态估计的基石算法。本文从机器人位置估计这一经典场景出发：

1. 原理层面
   ：梳理了卡尔曼滤波的五大核心公式，阐明了预测与更新的闭环逻辑。
2. Python层面
   ：利用浮点运算实现了算法原型，验证了滤波器对噪声的抑制效果和增益收敛特性。
3. Verilog层面
   ：设计了10级流水线的定点数实现，通过缩放因子策略在16位位宽下完成了滤波运算，达到了200ns/次迭代的实时性能。

两种实现方式各有其用：Python用于"想清楚"，在算法层面验证正确性；Verilog用于"跑得快"，在硬件层面实现实时处理。在实际工程中，通常先用Python/MATLAB进行算法仿真和参数整定，再将验证通过的算法移植到Verilog/VHDL进行硬件部署——这正是从算法到芯片的完整开发流程。

工程参考代码清单

Python仿真代码

见上文第三节完整代码，

Verilog完整工程文件