卡尔曼滤波是一种高效的递归滤波器，能够从一系列包含噪声的测量中估计动态系统的状态。它广泛应用于导航定位、目标跟踪、自动驾驶等领域。本文用Java完整实现卡尔曼滤波算法，讲解预测-更新双阶段模型、协方差矩阵传播与最优增益计算，帮助读者掌握这一经典状态估计算法。
一、算法原理
卡尔曼滤波的核心思想是融合两种信息来源：
- 系统模型预测：根据上一时刻的状态和控制输入，预测当前时刻的状态；
- 传感器测量：通过观测设备获取当前时刻的带噪声测量值。
通过最优增益矩阵，卡尔曼滤波将预测值与测量值加权融合，得到统计意义上的最优估计。
数学模型
状态转移方程：
x_k = F * x_{k-1} + B * u_k + w_k
观测方程：
z_k = H * x_k + v_k
其中：
– x_k 为时刻k的状态向量
– F 为状态转移矩阵
– B 为控制输入矩阵
– u_k 为控制向量
– w_k 为过程噪声，协方差为Q
– z_k 为观测向量
– H 为观测矩阵
– v_k 为观测噪声，协方差为R
预测阶段
x̂_k|k-1 = F * x̂_{k-1|k-1} + B * u_k
P_k|k-1 = F * P_{k-1|k-1} * F^T + Q
更新阶段
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^(-1)
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
P_k|k = (I - K_k * H) * P_k|k-1
二、Java实现
矩阵工具类
import java.util.Arrays;
/**
* 简单矩阵运算工具类
* 支持基本的矩阵乘法、转置、求逆（用于低维矩阵）
*/
public class Matrix {
private final double[][] data;
private final int rows;
private final int cols;
public Matrix(int rows, int cols) {
this.rows = rows;
this.cols = cols;
this.data = new double[rows][cols];
}
public Matrix(double[][] data) {
this.rows = data.length;
this.cols = data[0].length;
this.data = new double[rows][cols];
for (int i = 0; i < rows; i++) {
System.arraycopy(data[i], 0, this.data[i], 0, cols);
}
}
public double get(int i, int j) { return data[i][j]; }
public void set(int i, int j, double val) { data[i][j] = val; }
public int getRows() { return rows; }
public int getCols() { return cols; }
/**
* 矩阵乘法
*/
public Matrix multiply(Matrix other) {
if (this.cols != other.rows) {
throw new IllegalArgumentException("矩阵维度不匹配");
}
Matrix result = new Matrix(this.rows, other.cols);
for (int i = 0; i < this.rows; i++) {
for (int j = 0; j < other.cols; j++) {
double sum = 0;
for (int k = 0; k < this.cols; k++) {
sum += this.data[i][k] * other.data[k][j];
}
result.set(i, j, sum);
}
}
return result;
}
/**
* 矩阵转置
*/
public Matrix transpose() {
Matrix result = new Matrix(cols, rows);
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
result.set(j, i, data[i][j]);
}
}
return result;
}
/**
* 矩阵加法
*/
public Matrix add(Matrix other) {
if (this.rows != other.rows || this.cols != other.cols) {
throw new IllegalArgumentException("矩阵维度不匹配");
}
Matrix result = new Matrix(rows, cols);
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
result.set(i, j, this.data[i][j] + other.data[i][j]);
}
}
return result;
}
/**
* 矩阵减法
*/
public Matrix subtract(Matrix other) {
if (this.rows != other.rows || this.cols != other.cols) {
throw new IllegalArgumentException("矩阵维度不匹配");
}
Matrix result = new Matrix(rows, cols);
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
result.set(i, j, this.data[i][j] - other.data[i][j]);
}
}
return result;
}
/**
* 标量乘法
*/
public Matrix scale(double scalar) {
Matrix result = new Matrix(rows, cols);
for (int i = 0; i < rows; i++) {
for (int j = 0; j < cols; j++) {
result.set(i, j, data[i][j] * scalar);
}
}
return result;
}
/**
* 2x2矩阵求逆（简化实现）
*/
public Matrix inverse2x2() {
if (rows != 2 || cols != 2) {
throw new IllegalArgumentException("仅支持2x2矩阵求逆");
}
double a = data[0][0], b = data[0][1];
double c = data[1][0], d = data[1][1];
double det = a * d - b * c;
if (Math.abs(det) < 1e-10) {
throw new ArithmeticException("矩阵奇异，无法求逆");
}
Matrix result = new Matrix(2, 2);
result.set(0, 0, d / det);
result.set(0, 1, -b / det);
result.set(1, 0, -c / det);
result.set(1, 1, a / det);
return result;
}
/**
* 单位矩阵
*/
public static Matrix identity(int n) {
Matrix result = new Matrix(n, n);
for (int i = 0; i < n; i++) {
result.set(i, i, 1.0);
}
return result;
}
@Override
public String toString() {
StringBuilder sb = new StringBuilder();
for (int i = 0; i < rows; i++) {
sb.append("[");
for (int j = 0; j < cols; j++) {
sb.append(String.format("%.4f", data[i][j]));
if (j < cols - 1) sb.append(", ");
}
sb.append("]\n");
}
return sb.toString();
}
}
卡尔曼滤波器
/**
* 卡尔曼滤波器实现
* 支持任意维度的状态向量和观测向量
*/
public class KalmanFilter {
// 状态向量维度
private final int stateDim;
// 观测向量维度
private final int obsDim;
// 状态转移矩阵 F
private Matrix F;
// 控制输入矩阵 B
private Matrix B;
// 观测矩阵 H
private Matrix H;
// 过程噪声协方差 Q
private Matrix Q;
// 观测噪声协方差 R
private Matrix R;
// 当前状态估计 x̂
private Matrix x;
// 当前估计误差协方差 P
private Matrix P;
public KalmanFilter(int stateDim, int obsDim) {
this.stateDim = stateDim;
this.obsDim = obsDim;
// 初始化矩阵
this.F = Matrix.identity(stateDim);
this.B = new Matrix(stateDim, 1);
this.H = new Matrix(obsDim, stateDim);
this.Q = Matrix.identity(stateDim).scale(0.01);
this.R = Matrix.identity(obsDim).scale(0.1);
// 初始状态为0，协方差为单位矩阵
this.x = new Matrix(stateDim, 1);
this.P = Matrix.identity(stateDim);
}
/**
* 设置状态转移矩阵
*/
public void setF(Matrix F) {
if (F.getRows() != stateDim || F.getCols() != stateDim) {
throw new IllegalArgumentException("F矩阵维度错误");
}
this.F = F;
}
/**
* 设置控制输入矩阵
*/
public void setB(Matrix B) {
if (B.getRows() != stateDim) {
throw new IllegalArgumentException("B矩阵维度错误");
}
this.B = B;
}
/**
* 设置观测矩阵
*/
public void setH(Matrix H) {
if (H.getRows() != obsDim || H.getCols() != stateDim) {
throw new IllegalArgumentException("H矩阵维度错误");
}
this.H = H;
}
/**
* 设置过程噪声协方差
*/
public void setQ(Matrix Q) {
if (Q.getRows() != stateDim || Q.getCols() != stateDim) {
throw new IllegalArgumentException("Q矩阵维度错误");
}
this.Q = Q;
}
/**
* 设置观测噪声协方差
*/
public void setR(Matrix R) {
if (R.getRows() != obsDim || R.getCols() != obsDim) {
throw new IllegalArgumentException("R矩阵维度错误");
}
this.R = R;
}
/**
* 设置初始状态
*/
public void setInitialState(Matrix x0, Matrix P0) {
this.x = x0;
this.P = P0;
}
/**
* 预测步骤
* @param u 控制输入（可选，为null时忽略）
*/
public void predict(Matrix u) {
// x̂_k|k-1 = F * x̂_{k-1|k-1} + B * u
x = F.multiply(x);
if (u != null && B != null) {
x = x.add(B.multiply(u));
}
// P_k|k-1 = F * P_{k-1|k-1} * F^T + Q
P = F.multiply(P).multiply(F.transpose()).add(Q);
}
/**
* 更新步骤
* @param z 观测值
*/
public void update(Matrix z) {
// 计算卡尔曼增益
// K = P * H^T * (H * P * H^T + R)^(-1)
Matrix S = H.multiply(P).multiply(H.transpose()).add(R);
Matrix K = P.multiply(H.transpose()).multiply(S.inverse2x2());
// x̂_k|k = x̂_k|k-1 + K * (z - H * x̂_k|k-1)
Matrix y = z.subtract(H.multiply(x)); // 新息（观测残差）
x = x.add(K.multiply(y));
// P_k|k = (I - K * H) * P_k|k-1
Matrix I = Matrix.identity(stateDim);
P = I.subtract(K.multiply(H)).multiply(P);
}
/**
* 执行一次完整的预测-更新循环
*/
public void step(Matrix u, Matrix z) {
predict(u);
update(z);
}
public Matrix getState() { return x; }
public Matrix getCovariance() { return P; }
public static void main(String[] args) {
// 示例：一维匀速运动模型
// 状态向量：[位置, 速度]^T
// 观测：仅测量位置
KalmanFilter kf = new KalmanFilter(2, 1);
// 时间间隔
double dt = 1.0;
// 设置状态转移矩阵 F = [[1, dt], [0, 1]]
Matrix F = new Matrix(new double[][]{{1, dt}, {0, 1}});
kf.setF(F);
// 设置观测矩阵 H = [[1, 0]]
Matrix H = new Matrix(new double[][]{{1, 0}});
kf.setH(H);
// 设置过程噪声 Q
Matrix Q = new Matrix(new double[][]{{0.1, 0}, {0, 0.01}});
kf.setQ(Q);
// 设置观测噪声 R
Matrix R = new Matrix(new double[][]{{0.5}});
kf.setR(R);
// 初始状态
Matrix x0 = new Matrix(new double[][]{{0}, {1}}); // 位置0，速度1
Matrix P0 = Matrix.identity(2).scale(10);
kf.setInitialState(x0, P0);
// 模拟10步
System.out.println("卡尔曼滤波跟踪结果：");
System.out.println("步骤\t真实位置\t观测位置\t估计位置\t估计速度\t位置方差");
double truePos = 0;
double trueVel = 1;
java.util.Random rand = new java.util.Random(42);
for (int k = 1; k <= 10; k++) {
// 真实状态演进
truePos += trueVel * dt;
// 带噪声的观测
double measurement = truePos + rand.nextGaussian() * Math.sqrt(0.5);
Matrix z = new Matrix(new double[][]{{measurement}});
// 执行卡尔曼滤波
kf.step(null, z);
Matrix state = kf.getState();
Matrix cov = kf.getCovariance();
System.out.printf("%d\t%.3f\t\t%.3f\t\t%.3f\t\t%.3f\t\t%.4f%n",
k, truePos, measurement, state.get(0, 0), state.get(1, 0), cov.get(0, 0));
}
}
}
三、应用场景扩展
卡尔曼滤波可以扩展到多维场景。以下是一个二维目标跟踪的示例配置：
/**
* 二维目标跟踪配置示例
* 状态：[x, y, vx, vy]^T
* 观测：[x, y]^T
*/
public static KalmanFilter create2DTracker(double dt) {
KalmanFilter kf = new KalmanFilter(4, 2);
// F = [[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]
Matrix F = new Matrix(new double[][]{
{1, 0, dt, 0},
{0, 1, 0, dt},
{0, 0, 1, 0},
{0, 0, 0, 1}
});
kf.setF(F);
// H = [[1, 0, 0, 0], [0, 1, 0, 0]]
Matrix H = new Matrix(new double[][]{
{1, 0, 0, 0},
{0, 1, 0, 0}
});
kf.setH(H);
// Q：过程噪声
Matrix Q = Matrix.identity(4).scale(0.05);
kf.setQ(Q);
// R：观测噪声（GPS精度约5米）
Matrix R = Matrix.identity(2).scale(5.0);
kf.setR(R);
return kf;
}
四、复杂度分析
- 预测阶段：主要开销为矩阵乘法F×P×F^T，时间复杂度为O(n³)，n为状态维度；
- 更新阶段：主要开销为求逆运算，对于m维观测，复杂度为O(m³)；
- 整体：单次迭代复杂度为O(n³ + m³)。对于常见应用（n≤10, m≤5），计算量极小，完全满足实时性要求。
五、总结
本文用Java完整实现了卡尔曼滤波算法，核心要点包括：
- 预测-更新框架：清晰分离系统模型预测与观测融合两个步骤；
- 最优增益推导：通过最小化估计误差协方差，得到理论最优的卡尔曼增益；
- 矩阵运算实现：构建基础的矩阵工具类，支撑滤波器的核心计算；
- 多维扩展：从一维匀速模型自然扩展到二维目标跟踪等复杂场景。
卡尔曼滤波是状态估计领域的基石算法，理解它对于后续学习扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）以及粒子滤波等高级方法具有重要的铺垫意义。
