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

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

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

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

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

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

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

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

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

阶段	公式	含义
预测	$\hat{X}_k^- = A\hat{X}_{k-1} + BU_{k-1}$	先验状态估计：用上一时刻的最优估计预测当前状态
预测	$P_k^- = AP_{k-1}A^T + Q$	先验误差协方差：预测的不确定性会累积
更新	$K_k = \frac{P_k^-H^T}{HP_k^-H^T + R}$	卡尔曼增益：决定"更信预测还是更信测量"
更新	$\hat{X}_k = \hat{X}_k^- + K_k(Z_k - H\hat{X}_k^-)$	后验状态估计：用测量残差修正预测
更新	$P_k = (I - K_kH)P_k^-$	后验误差协方差：更新后的不确定性

其中：

$A$ 为状态转移矩阵，$B$ 为控制矩阵，$H$ 为观测矩阵
$Q$ 为过程噪声协方差，$R$ 为测量噪声协方差
$K_k$ 是卡尔曼增益，它是整个算法的"灵魂"：$P_k^-$ 越大（预测越不靠谱），$K_k$ 越大，越重视测量反馈。

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

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

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

我们用状态向量 $\vec{x}$ 来表示机器人的当前状态：

$\vec{x}=\left[ \begin{array}{c}{p} \\ {v}\end{array}\right] \\$

其中 $p$ 和 $v$ 分别为相对于绝对坐标系 $O_1$ 的位移。

那么我们要做的是，已知某时刻的最佳状态估计 $\hat{\mathbf{x}}_{k}$ ，预测下一时刻的最佳估计 $\hat{\mathbf{x}}_{k+1}$ 。

设计运动

我们为 Robo 设计一个匀加速直线运动。设初始状态向量为 $\vec{x_0}=\left[ \begin{array}{c}{0} &{1}\end{array}\right]^T $ ，并有一个加速指令性，使其以 $a = 0.1$ 的加速度做匀加速直线运动。那么根据匀加速直线运动的位移计算公式，任意时刻 $k$ 的位移 $p$ 与速度 $v$ 都可以由位移、速度、加速度递推得到：

$\begin{aligned} p_{k} &=p_{k-1}+v_{k-1} \times \Delta k+a\times \frac{\Delta k^{2}}{2} \\ v_{k} &=v_{k-1}+a \times \Delta k \end{aligned} \tag{3} $

将其转化为矩阵形式：

$\left[ \begin{array}{c}{p_{k}} \\ {v_{k}}\end{array}\right]=\left[ \begin{array}{cc}{1} & {\Delta k} \\ {0} & {1}\end{array}\right] \left[ \begin{array}{c}{p_{k-1}} \\ {v_{k-1}}\end{array}\right]+\left[ \begin{array}{c}{\frac{\Delta k^{2}}{2}} \\ {\Delta k}\end{array}\right] a \tag{4}$

$x_{t}=F_{t} x_{t-1}+B_{t} a \\$

我们取时间间隔 $\Delta k = 1$ ，由上式编程计算得到机器人的真实状态向量轨迹：

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)

位移真实值

速度真实值

传感器的观测值

我们有一台传感器 $O_2$ 放置于机器人的前方，它可以测得机器人位移 $s$ 和速度 $w$ 。当然，传感器自身本身有一定的噪声 $\xi$ ，其协方差为 $R$ 。由于传感器的信息是相对于自身坐标系 $O_2$ 的，而我们的状态向量 $\vec{x}$ 是相对于绝对坐标系 $O_1$ 的，二者的方向刚好相反，我们使用观测矩阵 $\mathbf{H}$ 来转换二者的关系。因此，相对于真实状态向量，传感器的观测向量可以表示如下：

$\left[ \begin{array}{c}{s} \\ {w}\end{array}\right]=\left[ \begin{array}{cc}{-1} & 0\\ {0} & {-1}\end{array}\right] \left[ \begin{array}{c}{p} \\ {v}\end{array}\right]+\left[ \begin{array}{c}{\xi_p} \\ {\xi_v}\end{array}\right] \tag{5}$

$z_{t}=H x_{t}+\xi \\$

我们设传感器噪声的协方差 $R$ 为单位阵(噪声方差为1)。根据上式编写程序，得到的传感器的观测值如下：

## 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个模块：

模块	功能	对应公式
`Kalman`	顶层模块，实例化子模块并连接信号	—
`Kalman_1`	预测状态 `X^-`	公式1
`Kalman_2_3`	预测协方差 `P^-` 并计算增益 `Kg`	公式2、3
`Kalman_4_5`	更新状态 `X` 和协方差 `P`	公式4、5
`Kalman_ctrl`	数据控制器，从DPRAM读取测量值	—

以及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实现	Verilog实现
数据表示	IEEE 754 双精度浮点（64位）	16位定点整数 + 缩放因子
运算精度	高（~15位有效数字）	有限（受限于位宽和缩放策略）
执行速度	受限于CPU，~μs级	50MHz时钟，200ns/次迭代
资源消耗	内存（MB级）	LUT/FF/DSP（硬件资源）
开发难度	低，代码简洁	高，需处理时序、缩放、位宽
适用场景	算法验证、仿真、离线分析	实时嵌入式、FPGA加速、ASIC
可移植性	跨平台	依赖具体FPGA/ASIC工艺

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

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

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

六、总结

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

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

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