FA ：formulas and algorithm, FF ：fusion and fitting, FKF：Federated Kalman Filter
二、联邦滤波简述
1. 核心定义与背景
联邦滤波是一种分布式多传感器信息融合滤波算法，由卡尔曼滤波扩展而来，核心思想是：
将全局滤波任务分解为多个子滤波器（对应不同传感器 / 子系统），子滤波器独立运行完成局部状态估计，再通过 "信息分配 / 融合" 策略将子滤波器结果合并为全局估计，同时支持子滤波器的 "信息重置" 以保证全局一致性。区别于集中式融合（所有传感器数据汇总到中心节点统一滤波），联邦滤波兼具分布式的灵活性 （子滤波器独立工作）和全局最优性（融合后接近集中式性能），是多传感器融合领域的经典框架。
2. 依托的基本原理与定理
联邦滤波的数学根基是以下核心理论：
| 基础理论 | 核心作用 |
|---|---|
| 卡尔曼滤波（KF/UKF/EKF） | 子滤波器和主滤波器的基础滤波框架，提供 "预测 - 更新" 的递推估计能力 |
| 信息论（信息矩阵 / 信息向量） | 联邦滤波的核心融合工具，信息矩阵 Y=P−1（协方差的逆），信息向量 y=P−1x^，融合本质是信息的加权求和 |
| 贝叶斯估计定理 | 多源信息融合的理论基础，全局估计是各子滤波器估计的后验概率加权组合 |
| 正交投影原理 | 卡尔曼滤波的本质是状态在观测空间的正交投影，联邦滤波的信息融合满足正交投影的最优性 |
| 信息守恒原理 | 联邦滤波的 "信息分配 / 重置" 需满足全局信息总量守恒，保证滤波稳定性 |
3. 核心机器原理与核心公式
子滤波器：对应单个传感器 / 子系统，独立完成局部状态估计，输出局部状态 x^i 和协方差 Pi；
主滤波器：负责接收子滤波器结果、融合得到全局估计、向子滤波器分配信息（保证一致性）；
信息分配系数：βi（i=0 为主滤波器，i=1..n 为子滤波器），满足 ∑i=0nβi=1，βi 决定子滤波器可使用的全局信息比例。（1）核心公式（基于信息矩阵的融合）
定义：信息矩阵与信息向量
- 信息矩阵：Y=P−1Y=P^{−1}Y=P−1（协方差的逆，代表 "信息含量"，值越大信息越丰富）；
- 信息向量：y=Yx^=P−1x^y=Y\hat{x}=P^{-1}\hat{x}y=Yx^=P−1x^。
步骤 1：信息分配（主→子）
主滤波器将全局信息按分配系数 βiβ_iβi 分配给子滤波器 iii：
Yi0=βiYgY_i^0=β_iY_gYi0=βiYg
yi0=βiygy_i^0=β_iy_gyi0=βiyg
其中：
- Yg、ygY_g、y_gYg、yg：全局信息矩阵 / 向量；
- Yi0、yi0Y_i^0、y_i^0Yi0、yi0：分配给子滤波器 i 的先验信息矩阵 / 向量；
- 子滤波器初始协方差 / 状态：Pi0=(Yi0)−1，x^i0=(Yi0)−1yi0P_i^0=(Y_i^0)^{−1}，\hat{x}_i^0=(Y_i^0)^{−1}y_i^0Pi0=(Yi0)−1，x^i0=(Yi0)−1yi0。
步骤 2：子滤波器局部估计
子滤波器 iii 基于本地传感器数据，按卡尔曼滤波完成局部更新：
x^i=Pi((Pi0)−1x^i0+HiTRi−1zi)\hat{x}_i=P_i((P_i^0)^{-1}\hat{x}_i^0+\mathbf{H}_i^TR_i^{-1z_i})x^i=Pi((Pi0)−1x^i0+HiTRi−1zi)
Pi=((Pi0)−1+HiTRi−1Hi)−1P_i=((P_i^0)^{−1}+\mathbf{H}_i^TR_i^{−1}H_i)^{−1}Pi=((Pi0)−1+HiTRi−1Hi)−1
（本质是卡尔曼滤波的更新公式，Pi0P_i^0Pi0 为子滤波器先验协方差）
步骤 3：全局信息融合（子→主）
主滤波器将所有子滤波器的信息加权求和，得到全局估计：
Yg=∑i=1nYiY_g=\sum_{i=1}^nY_iYg=∑i=1nYi
yg=∑i=1nyiy_g=\sum_{i=1}^{n}y_iyg=∑i=1nyi
x^g=Yg−1yg\hat{x}_g=Y_g^{-1}y_gx^g=Yg−1yg
Pg=Yg−1P_g=Y_g^{-1}Pg=Yg−1
步骤 4：信息重置（可选，保证一致性）
将全局信息按 βiβ_iβi 反馈给子滤波器，重置子滤波器的先验信息：
Yi0=βiYgY_i^0=β_iY_gYi0=βiYg
yi0=βiygy_i^0=β_iy_gyi0=βiyg
x^i0=x^g，Pi0=Pgβi\hat{x}_i^0=\hat{x}_g，P_i^0=\frac{P_g}{β_i}x^i0=x^g，Pi0=βiPg
4. 联邦滤波的操作步骤（工程落地流程）
联邦滤波的核心是 "分配 - 局部滤波 - 融合 - 重置" 的闭环，具体操作步骤如下：
步骤 1：系统建模与初始化
- 定义全局状态向量 xxx（如无人机的位置、速度、姿态）；
- 划分子滤波器：按传感器类型（GPS、IMU、视觉）或子系统（位置、速度、姿态）划分，确定子滤波器数量 nnn；
- 初始化全局状态 x^g0\hat{x}_g^0x^g0 和协方差 Pg0P_g^0Pg0；
- 设定信息分配系数 βiβ_iβi（如均等分配：βi=1/(n+1)β_i=1/(n+1)βi=1/(n+1)，β0β_0β0 为主滤波器预留）。
步骤 2：信息分配
- 主滤波器计算全局信息矩阵 Yg=(Pg)−1Yg=(P_g)^{−1}Yg=(Pg)−1、信息向量 yg=Ygx^gy_g=Y_g\hat{x}_gyg=Ygx^g；
- 按 βiβ_iβi 将信息分配给各子滤波器，得到子滤波器的先验状态 x^i0\hat{x}_i^0x^i0 和协方差 Pi0P_i^0Pi0。
步骤 3：子滤波器局部滤波
每个子滤波器独立执行：
- 预测：基于本地系统模型完成状态预测（x^i−、Pi−\hat{x}_i^−、P_i^−x^i−、Pi−）；
- 更新：结合本地传感器观测值，完成局部状态更新（x^i、Pi\hat{x}_i、P_ix^i、Pi）；
- 输出：向主滤波器上报局部估计结果（\hat{x}_i、P_i）。
步骤 4：全局信息融合
主滤波器接收所有子滤波器的结果，按信息融合公式计算全局状态 x^g\hat{x}_gx^g 和协方差 PgP_gPg。
步骤 5：信息重置（可选）
将全局信息按 βiβ_iβi 反馈给子滤波器，重置子滤波器的先验信息，保证下一时刻子滤波器与全局一致。
步骤 6：迭代执行
重复步骤 2-5，完成多传感器的实时融合滤波。
5. 应用场景
联邦滤波因分布式、容错性、可扩展性 的优势，广泛应用于多传感器融合场景：
表格
| 应用领域 | 具体场景 | 子滤波器划分方式 |
|---|---|---|
| 无人机导航 | GPS+IMU + 视觉里程计融合 | 子滤波器 1（GPS）、子滤波器 2（IMU）、子滤波器 3（视觉） |
| 车辆自动驾驶 | 激光雷达 + 摄像头 + 毫米波雷达 + 惯导 | 按传感器类型划分子滤波器 |
| 卫星导航 | 多星座（GPS + 北斗 + GLONASS）融合 | 按卫星星座划分子滤波器 |
| 机器人 SLAM | 激光 SLAM + 视觉 SLAM + 里程计 | 按 SLAM 模块划分子滤波器 |
| 船舶导航 | 惯导 + GPS + 电子海图 | 按导航设备划分子滤波器 |
6. 优缺点对比（与集中式 / 分布式融合）
| 融合方式 | 优点 | 缺点 |
|---|---|---|
| 集中式融合 | 理论最优精度、无信息损失 | 计算量大、容错性差（单个传感器故障导致全局失效）、扩展性差 |
| 分布式融合（非联邦） | 计算量小、容错性好 | 精度低于集中式、无全局信息一致性保证 |
| 联邦滤波 | 1. 分布式架构：子滤波器独立运行，计算量分散；2. 容错性强：单个子滤波器故障不影响全局；3. 扩展性好：新增传感器仅需添加子滤波器；4. 精度接近集中式；5. 信息可重置：保证全局一致性 | 1. 信息分配系数需手动调参，无统一最优准则；2. 略增加主滤波器的信息融合计算量；3. 对信息矩阵求逆存在数值稳定性风险 |
7. Python 代码实现（双传感器联邦滤波示例）
以 "二维位置跟踪" 为例，融合 GPS（子滤波器 1）和视觉传感器（子滤波器 2）的观测，实现联邦滤波：
bash
import numpy as np
import matplotlib.pyplot as plt
class SubKalmanFilter:
"""子卡尔曼滤波器（单个传感器的局部滤波）"""
def __init__(self, F, H, Q, R, x0, P0):
self.F = F # 状态转移矩阵
self.H = H # 观测矩阵
self.Q = Q # 过程噪声协方差
self.R = R # 观测噪声协方差
self.x_hat = x0 # 局部状态估计
self.P = P0 # 局部协方差
def predict(self):
"""预测阶段"""
self.x_hat = self.F @ self.x_hat
self.P = self.F @ self.P @ self.F.T + self.Q
def update(self, z):
"""更新阶段"""
# 卡尔曼增益
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
# 更新状态和协方差
self.x_hat += K @ (z - self.H @ self.x_hat)
I = np.eye(len(self.P))
self.P = (I - K @ self.H) @ self.P
class FederatedKalmanFilter:
"""联邦卡尔曼滤波器（主滤波器+子滤波器）"""
def __init__(self, n_sub, F, H_list, Q_list, R_list, x0, P0, beta_list):
"""
:param n_sub: 子滤波器数量
:param F: 全局状态转移矩阵
:param H_list: 各子滤波器观测矩阵列表
:param Q_list: 各子滤波器过程噪声协方差列表
:param R_list: 各子滤波器观测噪声协方差列表
:param x0: 全局初始状态
:param P0: 全局初始协方差
:param beta_list: 信息分配系数列表（长度n_sub，和为1）
"""
self.n_sub = n_sub
self.beta_list = beta_list # 信息分配系数
self.F = F
# 全局状态和协方差
self.x_g = x0
self.P_g = P0
# 初始化子滤波器
self.sub_filters = []
for i in range(n_sub):
# 按信息分配系数初始化子滤波器先验协方差和状态
P_i0 = self.P_g / beta_list[i] # 子滤波器先验协方差
x_i0 = x0.copy() # 子滤波器先验状态
sub_filter = SubKalmanFilter(F, H_list[i], Q_list[i], R_list[i], x_i0, P_i0)
self.sub_filters.append(sub_filter)
def information_allocation(self):
"""信息分配：主滤波器向子滤波器分配信息"""
for i in range(self.n_sub):
# 重置子滤波器先验状态和协方差
self.sub_filters[i].x_hat = self.x_g.copy()
self.sub_filters[i].P = self.P_g / self.beta_list[i]
def sub_filter_update(self, z_list):
"""子滤波器局部滤波"""
for i in range(self.n_sub):
self.sub_filters[i].predict() # 预测
self.sub_filters[i].update(z_list[i]) # 观测更新
def global_fusion(self):
"""全局信息融合：子滤波器结果融合为全局估计"""
# 计算全局信息矩阵和信息向量
Y_g = np.zeros_like(self.P_g) # 信息矩阵 Y = P^-1
y_g = np.zeros_like(self.x_g) # 信息向量 y = Y@x
for i in range(self.n_sub):
Y_i = np.linalg.inv(self.sub_filters[i].P)
y_i = Y_i @ self.sub_filters[i].x_hat
Y_g += Y_i
y_g += y_i
# 全局状态和协方差
self.P_g = np.linalg.inv(Y_g)
self.x_g = self.P_g @ y_g
def step(self, z_list):
"""联邦滤波一步迭代"""
self.information_allocation() # 信息分配
self.sub_filter_update(z_list) # 子滤波器局部更新
self.global_fusion() # 全局融合
return self.x_g, self.P_g
# ---------------------- 测试联邦滤波 ----------------------
if __name__ == "__main__":
# 1. 系统参数
dt = 0.1 # 时间步长
T = 50 # 总步数
n_sub = 2 # 子滤波器数量（GPS+视觉）
# 全局状态：[x, y, vx, vy]（位置x/y，速度vx/vy）
F = np.array([
[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# 子滤波器1（GPS）：观测x,y位置
H1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
Q1 = np.diag([0.01, 0.01, 0.1, 0.1]) # 过程噪声
R1 = np.diag([1.0, 1.0]) # 观测噪声（GPS噪声较大）
# 子滤波器2（视觉）：观测x,y位置
H2 = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
Q2 = np.diag([0.01, 0.01, 0.1, 0.1]) # 过程噪声
R2 = np.diag([0.2, 0.2]) # 观测噪声（视觉噪声较小）
H_list = [H1, H2]
Q_list = [Q1, Q2]
R_list = [R1, R2]
# 信息分配系数（视觉权重更高，因为噪声小）
beta1 = 0.3 # GPS子滤波器
beta2 = 0.7 # 视觉子滤波器
beta_list = [beta1, beta2]
# 初始状态和协方差
x0 = np.array([0, 0, 1, 1]) # 初始状态：x=0,y=0,vx=1,vy=1
P0 = np.diag([1, 1, 1, 1]) # 初始协方差
# 2. 生成真实状态和观测值
x_true = np.zeros((T, 4))
x_true[0] = x0
z1_meas = np.zeros((T, 2)) # GPS观测
z2_meas = np.zeros((T, 2)) # 视觉观测
for k in range(1, T):
# 真实状态转移（匀速运动+过程噪声）
process_noise = np.random.multivariate_normal([0,0,0,0], Q1)
x_true[k] = F @ x_true[k-1] + process_noise
# 生成观测值
z1_meas[k] = H1 @ x_true[k] + np.random.multivariate_normal([0,0], R1)
z2_meas[k] = H2 @ x_true[k] + np.random.multivariate_normal([0,0], R2)
# 3. 初始化联邦滤波
ff = FederatedKalmanFilter(n_sub, F, H_list, Q_list, R_list, x0, P0, beta_list)
# 4. 联邦滤波迭代
x_g_est = np.zeros((T, 4)) # 全局估计结果
x1_est = np.zeros((T, 4)) # GPS子滤波器估计
x2_est = np.zeros((T, 4)) # 视觉子滤波器估计
for k in range(T):
z_list = [z1_meas[k], z2_meas[k]]
x_g, P_g = ff.step(z_list)
x_g_est[k] = x_g
x1_est[k] = ff.sub_filters[0].x_hat
x2_est[k] = ff.sub_filters[1].x_hat
# 5. 结果可视化
plt.figure(figsize=(12, 8))
# 位置轨迹对比
plt.subplot(2, 1, 1)
plt.plot(x_true[:, 0], x_true[:, 1], label='真实轨迹', linewidth=2)
plt.plot(x_g_est[:, 0], x_g_est[:, 1], label='联邦滤波全局估计', linestyle='--', linewidth=2)
plt.plot(x1_est[:, 0], x1_est[:, 1], label='GPS子滤波器', linestyle=':', alpha=0.7)
plt.plot(x2_est[:, 0], x2_est[:, 1], label='视觉子滤波器', linestyle='-.', alpha=0.7)
plt.xlabel('X位置')
plt.ylabel('Y位置')
plt.legend()
plt.title('二维位置跟踪轨迹对比')
# X位置随时间变化
plt.subplot(2, 1, 2)
plt.plot(range(T), x_true[:, 0], label='真实X位置')
plt.plot(range(T), x_g_est[:, 0], label='联邦滤波全局估计', linestyle='--')
plt.plot(range(T), z1_meas[:, 0], label='GPS观测', alpha=0.5)
plt.plot(range(T), z2_meas[:, 0], label='视觉观测', alpha=0.5)
plt.xlabel('时间步')
plt.ylabel('X位置')
plt.legend()
plt.title('X位置随时间变化')
plt.tight_layout()
plt.show()代码说明
- 子滤波器类：实现单个传感器的卡尔曼滤波（预测 + 更新），独立处理本地观测；
- 联邦滤波器类 ：
- 信息分配：按系数 βi 将全局信息分配给子滤波器，重置子滤波器先验；
- 局部滤波：各子滤波器独立完成预测和更新；
- 全局融合：将子滤波器的信息矩阵 / 向量求和，得到全局状态和协方差；
- 测试案例：模拟 GPS + 视觉双传感器的二维位置跟踪，对比真实轨迹、联邦全局估计、子滤波器估计的效果；
- 关键细节：视觉传感器噪声更小，因此分配更高的信息系数（β2=0.7），提升全局估计精度。
三、总结
- 核心原理：联邦滤波是分布式多传感器融合框架，核心是 "信息分配 - 局部滤波 - 全局融合 - 信息重置"，依托信息论和卡尔曼滤波实现全局最优估计，信息矩阵（协方差逆）是融合的核心工具；
- 操作关键：
- 子滤波器按传感器 / 子系统划分，独立运行；
- 信息分配系数需根据传感器精度调整（精度越高，系数越大）；
- 全局融合通过信息矩阵求和实现，保证最优性；
- 核心优势：分布式架构、容错性强、扩展性好、精度接近集中式；
- 适用场景：多传感器融合的导航、定位、跟踪场景（无人机、自动驾驶、机器人），是工程中平衡精度与工程实用性的优选方案。