卡尔曼滤波与扩展卡尔曼滤波原理及工程实践-AI智能范式网

卡尔曼滤波与扩展卡尔曼滤波原理及工程实践

卡尔曼滤波扩展卡尔曼滤波状态估计

|难度：普通|阅读约 29 分钟

1. 卡尔曼滤波与扩展卡尔曼滤波基础原理

卡尔曼滤波（Kalman Filter, KF）本质上是一种递归估计算法，它通过"预测-更新"的闭环机制实现对系统状态的最优估计。想象一下你在雾天开车，GPS信号时有时无，但你的大脑却能根据车速和方向盘转角不断修正对当前位置的判断——这就是卡尔曼滤波的直观体现。

KF的核心数学表达包含五个关键方程：

状态预测方程：
x̂ₖ⁻ = Fₖx̂ₖ₋₁ + Bₖuₖ
这个方程根据上一时刻的状态估计和控制输入，预测当前时刻的状态。Fₖ是状态转移矩阵，描述了系统如何随时间演化。
协方差预测方程：
Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ
它量化了预测的不确定性，Qₖ是过程噪声协方差矩阵。
卡尔曼增益计算：
Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹
这个神奇的增益决定了我们应该多大程度上信任新的观测数据。
状态更新方程：
x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hₖx̂ₖ⁻)
通过融合预测和观测，得到最优估计。
协方差更新方程：
Pₖ = (I - KₖHₖ)Pₖ⁻
更新后的不确定性评估。

注意：实现KF时，协方差矩阵P的初始化对收敛速度有重要影响。通常可以设为对角矩阵，对角线元素根据各状态变量的初始不确定程度设置。

当系统存在非线性特性时，EKF通过一阶泰勒展开在估计点附近进行局部线性化。具体来说：

状态转移函数f和观测函数h的雅可比矩阵：
Fₖ ≈ ∂f/∂x|ₓ₌x̂ₖ₋₁
Hₖ ≈ ∂h/∂x|ₓ₌x̂ₖ⁻
预测步骤：
x̂ₖ⁻ = f(x̂ₖ₋₁, uₖ)
Pₖ⁻ = FₖPₖ₋₁Fₖᵀ + Qₖ
更新步骤：
Kₖ = Pₖ⁻Hₖᵀ(HₖPₖ⁻Hₖᵀ + Rₖ)⁻¹
x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - h(x̂ₖ⁻))
Pₖ = (I - KₖHₖ)Pₖ⁻

在实际应用中，EKF的线性化误差会导致"滤波发散"现象。我曾经在一个无人机项目中遇到过这种情况：当无人机做剧烈机动时，传统EKF的位置估计会出现明显偏差。后来通过以下方法解决了问题：

限制状态更新步长，防止线性化区域过大
加入自适应机制，在机动时自动增大过程噪声Q
对雅可比矩阵进行数值稳定性处理

需要模型API调用？免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。

2. 惯性导航系统中的KF/EKF实现

2.1 INS误差建模关键点

惯性导航系统的误差主要来自IMU的测量误差和积分过程的累积误差。一个完整的INS误差状态通常包括：

姿态误差：3个自由度（俯仰、横滚、偏航）
速度误差：3个分量（北、东、天）
位置误差：3个分量（纬度、经度、高度）
IMU误差：陀螺零偏(3)、加速度计零偏(3)、刻度因子误差(6)

这样总共18维的状态向量，其动力学方程推导需要考虑：

姿态误差动力学：
δθ̇ = -ω×δθ + δω - Cₙᵇε
其中ω是角速率，ε是陀螺零偏，Cₙᵇ是导航系到机体系的旋转矩阵。
速度误差动力学：
δv̇ = -(ωₑₙₙ + 2ωₑₙₑ)×δv + f×δθ + Cₙᵇ∇
f是比力测量值，∇是加速度计零偏。
位置误差动力学：
δL̇ = δv_N/(R_N + h)
δλ̇ = δv_E/((R_E + h)cosL)
δḣ = δv_D

2.2 典型INS/GNSS松耦合实现

在Matlab中实现INS/GNSS松耦合导航系统时，我通常采用以下框架：

matlab复制% 初始化
x = zeros(15,1); % 状态向量 [姿态误差(3); 速度误差(3); 位置误差(3); 陀螺零偏(3); 加速度零偏(3)]
P = diag([deg2rad(1)*ones(1,3), 0.1*ones(1,3), 10*ones(1,3), 0.01*ones(1,3), 0.001*ones(1,3)]).^2;
Q = diag([1e-6*ones(1,3), 1e-5*ones(1,3), 1e-10*ones(1,3), 1e-8*ones(1,3), 1e-6*ones(1,3)]);
R = diag([1, 1, 2.5]).^2; % GNSS位置精度

while true
    % IMU数据到达
    [gyro, accel] = readIMU();
    
    % 预测步骤
    [x, P] = insPredict(x, P, gyro, accel, dt, Q);
    
    % GNSS数据到达
    if gpsUpdateAvailable()
        z_gnss = readGPS();
        
        % 更新步骤
        H = [zeros(3,6), eye(3), zeros(3,6)];
        z_pred = H * x;
        [x, P] = ekfUpdate(x, P, z_gnss, z_pred, H, R);
    end
end

实操技巧：在实际系统中，IMU和GNSS的数据到达频率不同（通常IMU 100-200Hz，GNSS 1-10Hz），需要妥善处理多速率问题。我通常采用"IMU驱动"的架构，只在GNSS数据到达时执行更新步骤。

2.3 INS初始对准的实用方法

INS的初始对准精度直接影响后续导航性能。在车辆应用中，我总结出以下实用方法：

静态粗对准：
- 利用加速度计测量重力矢量估计俯仰和横滚
- 陀螺测量地球自转分量估计方位角（低精度IMU通常不可行）
- 持续时间通常30-60秒
动态精对准：
- 车辆直线行驶时，通过GNSS航向校准IMU方位
- 采用双天线GNSS系统可提供更高精度的航向基准
- 典型精度：水平姿态0.1°，方位0.3°（消费级IMU）

在Matlab中实现静态粗对准的代码示例：

matlab复制function att = coarseAlignment(imu_data)
    % 取平均值减少噪声影响
    acc_mean = mean(imu_data.acc, 2);
    gyro_mean = mean(imu_data.gyro, 2);
    
    % 计算俯仰和横滚
    pitch = atan2(-acc_mean(1), sqrt(acc_mean(2)^2 + acc_mean(3)^2));
    roll = atan2(acc_mean(2), acc_mean(3));
    
    % 方位角估计（仅适用于高精度IMU）
    omega_ie = 7.292115e-5; % 地球自转角速率
    L = imu_data.lat * pi/180; % 纬度
    gyro_bias = gyro_mean - [omega_ie*cos(L); 0; -omega_ie*sin(L)];
    yaw = atan2(gyro_bias(2), gyro_bias(1));
    
    att = [roll; pitch; yaw]; % 欧拉角输出
end

3. GNSS导航中的滤波技术实现

3.1 GNSS单点定位的EKF实现

GNSS伪距观测模型是非线性的，非常适合EKF处理。伪距观测方程可以表示为：

ρⁱ = √((xⁱ - x)² + (yⁱ - y)² + (zⁱ - z)²) + c·δt + εⁱ

其中：

(xⁱ,yⁱ,zⁱ)是第i颗卫星的位置
(x,y,z)是接收机位置
δt是接收机钟差
εⁱ包含电离层延迟、对流层延迟、多径等误差

在Matlab中实现GNSS单点定位EKF的关键步骤：

matlab复制% 初始化
x = [0; 0; 0; 0]; % [x; y; z; 接收机钟差] 
P = diag([1e6, 1e6, 1e6, 1e6]); % 初始不确定度
Q = diag([1, 1, 1, 1e-4]); % 过程噪声
R = 25^2 * eye(n_sv); % 观测噪声，假设25m精度

while true
    % 预测步骤（简单随机游走模型）
    x_pred = x;
    P_pred = P + Q;
    
    % GNSS观测到达
    [sv_pos, pseudoranges] = readGNSS();
    
    % 计算预测伪距和观测矩阵H
    n_sv = size(sv_pos,1);
    h = zeros(n_sv,1);
    H = zeros(n_sv,4);
    for i = 1:n_sv
        dx = x_pred(1) - sv_pos(i,1);
        dy = x_pred(2) - sv_pos(i,2);
        dz = x_pred(3) - sv_pos(i,3);
        dist = sqrt(dx^2 + dy^2 + dz^2);
        h(i) = dist + x_pred(4);
        H(i,:) = [dx/dist, dy/dist, dz/dist, 1];
    end
    
    % EKF更新
    K = P_pred * H' / (H * P_pred * H' + R);
    x = x_pred + K * (pseudoranges - h);
    P = (eye(4) - K * H) * P_pred;
end

避坑指南：在实际实现中，要特别注意数值稳定性问题。我遇到过由于卫星几何分布不佳导致H矩阵条件数过大的情况，解决方法包括：

加入正则化项

使用奇异值分解(SVD)代替直接求逆

剔除仰角过低的卫星

3.2 紧耦合GNSS/INS实现要点

紧耦合系统直接处理原始伪距和多普勒观测值，相比松耦合有更好的抗干扰能力。其关键创新点在于：

状态向量扩充：
x = [δθ; δv; δp; ε; ∇; δt; δf]
新增接收机钟差δt和钟漂δf
观测模型：
- 伪距观测：与INS预测位置比较
- 多普勒观测：与INS预测速度比较
模糊度处理：
- 对于载波相位观测，需要估计整周模糊度
- 可采用LAMBDA方法或部分固定策略

在Matlab中实现紧耦合系统时，观测矩阵H的构建是关键：

matlab复制function [H_pr, H_dopp] = buildTightCouplingH(sv_pos, sv_vel, x_ins, lever_arm)
    n_sv = size(sv_pos,1);
    H_pr = zeros(n_sv,15+2); % 15个INS状态+钟差+钟漂
    H_dopp = zeros(n_sv,15+2);
    
    C_nb = euler2dcm(x_ins(1:3)); % 姿态转旋转矩阵
    v_ins = x_ins(4:6);
    p_ins = x_ins(7:9);
    
    for i = 1:n_sv
        % 考虑杆臂效应
        p_ant = p_ins + C_nb' * lever_arm;
        v_ant = v_ins + cross(x_ins(1:3), C_nb' * lever_arm);
        
        % 几何距离和视线向量
        dx = sv_pos(i,1) - p_ant(1);
        dy = sv_pos(i,2) - p_ant(2);
        dz = sv_pos(i,3) - p_ant(3);
        dist = sqrt(dx^2 + dy^2 + dz^2);
        los = [dx; dy; dz] / dist;
        
        % 伪距观测矩阵
        H_pr(i,1:3) = -los' * skewSymmetric(C_nb' * lever_arm);
        H_pr(i,4:6) = zeros(1,3);
        H_pr(i,7:9) = los';
        H_pr(i,16) = 1; % 钟差
        
        % 多普勒观测矩阵
        relative_vel = sv_vel(i,:)' - v_ant;
        H_dopp(i,1:3) = -los' * skewSymmetric(C_nb' * lever_arm) ...
                         + (relative_vel' / dist) * skewSymmetric(C_nb' * lever_arm);
        H_dopp(i,4:6) = los';
        H_dopp(i,7:9) = zeros(1,3);
        H_dopp(i,16) = 0;
        H_dopp(i,17) = 1; % 钟漂
    end
end

4. 目标跟踪中的自适应EKF技术

4.1 机动目标跟踪的挑战与解决方案

当目标进行机动（如转弯、加速）时，传统EKF会出现滞后甚至失跟。在我的雷达跟踪项目中，通过以下自适应机制解决了这个问题：

多模型交互（IMM）：
- 并行运行多个EKF，每个对应一种运动模型（匀速、匀加速、转弯等）
- 根据模型概率动态加权融合输出
自适应噪声调整：
- 监测新息序列（观测残差）
- 当新息持续超出阈值时，增大过程噪声Q
量测驱动采样：
- 根据最新观测调整状态转移模型参数

Matlab实现IMM的关键代码结构：

matlab复制% 初始化三个模型：匀速(CV)、匀加速(CA)、协调转弯(CT)
filters = {initCVFilter(), initCAFilter(), initCTFilter()};
model_prob = [0.8; 0.1; 0.1]; % 初始概率
transition_matrix = [0.9 0.05 0.05; 
                    0.1 0.8 0.1; 
                    0.1 0.1 0.8]; % 模型转移概率

while true
    % 1. 模型交互
    mixed_prob = transition_matrix' * model_prob;
    for i = 1:3
        filters{i}.x = zeros(size(filters{i}.x));
        for j = 1:3
            mix_prob_ij = transition_matrix(j,i) * model_prob(j) / mixed_prob(i);
            filters{i}.x = filters{i}.x + mix_prob_ij * filters{j}.x;
        end
    end
    
    % 2. 各模型独立预测和更新
    z = getRadarMeasurement();
    for i = 1:3
        filters{i} = predict(filters{i});
        filters{i} = update(filters{i}, z);
        
        % 计算模型似然
        S = filters{i}.H * filters{i}.P * filters{i}.H' + filters{i}.R;
        innov = z - filters{i}.H * filters{i}.x;
        filters{i}.likelihood = exp(-0.5 * innov' / S * innov) / sqrt(det(2*pi*S));
    end
    
    % 3. 模型概率更新
    total_prob = sum([filters{:}.likelihood] .* mixed_prob');
    model_prob = ([filters{:}.likelihood] .* mixed_prob') / total_prob;
    
    % 4. 输出融合
    x_est = zeros(size(filters{1}.x));
    for i = 1:3
        x_est = x_est + model_prob(i) * filters{i}.x;
    end
end

4.2 雷达目标跟踪中的非线性处理

雷达测量通常提供目标的距离r、方位角θ和仰角φ，这些量测与笛卡尔坐标系下的目标位置(x,y,z)之间存在非线性关系：

x = r·cosθ·cosφ
y = r·sinθ·cosφ
z = r·sinφ

在EKF中处理这种非线性时，需要特别注意：

笛卡尔坐标系下的匀速运动模型是线性的，但观测模型非线性
极坐标系下的运动模型是非线性的，但观测模型是线性的

我通常采用"笛卡尔状态+极坐标观测"的方案，其雅可比矩阵计算如下：

matlab复制function H = radarJacobian(x)
    r = sqrt(x(1)^2 + x(2)^2 + x(3)^2);
    H = zeros(3,6); % 假设状态是[x;vx; y;vy; z;vz]
    
    % 距离对状态的偏导
    H(1,1) = x(1)/r;
    H(1,3) = x(2)/r;
    H(1,5) = x(3)/r;
    
    % 方位角对状态的偏导
    H(2,1) = -x(2)/(x(1)^2 + x(2)^2);
    H(2,3) = x(1)/(x(1)^2 + x(2)^2);
    
    % 仰角对状态的偏导
    H(3,1) = -x(1)*x(3)/(r^2 * sqrt(x(1)^2 + x(2)^2));
    H(3,3) = -x(2)*x(3)/(r^2 * sqrt(x(1)^2 + x(2)^2));
    H(3,5) = sqrt(x(1)^2 + x(2)^2)/r^2;
end

经验分享：在低信噪比环境下，我发现以下技巧能显著提升跟踪性能：

对雷达原始数据进行聚类处理，剔除杂波

使用概率数据关联(PDA)代替最近邻关联

对低仰角目标适当增大观测噪声R

引入径向速度观测约束

5. 地形参考导航的工程实现

5.1 地形高度匹配算法选择

地形参考导航的核心是地形高度匹配(TERCOM)算法，常见的有：

均值差异算法(MDA)：
J = 1/N ∑|h_m - h_s|
计算简单但对异常值敏感
互相关算法(CCA)：
J = ∑(h_m - h̄_m)(h_s - h̄_s)
对地形起伏更敏感
最小二乘算法(LS)：
J = ∑(h_m - h_s)²
计算量适中，性能均衡

在Matlab中实现MDA算法的示例：

matlab复制function [pos_est, best_score] = tercomMDA(terrain_map, height_meas, search_area)
    [rows, cols] = size(terrain_map);
    best_score = inf;
    pos_est = [0, 0];
    
    % 遍历搜索区域
    for i = max(1,search_area(1,1)):min(rows,search_area(1,2))
        for j = max(1,search_area(2,1)):min(cols,search_area(2,2))
            % 提取候选地形剖面
            map_patch = terrain_map(i:i+length(height_meas)-1, j);
            
            % 计算匹配代价
            score = mean(abs(map_patch - height_meas));
            
            % 更新最佳匹配
            if score < best_score
                best_score = score;
                pos_est = [i, j];
            end
        end
    end
end

5.2 SITAN算法的EKF实现

SITAN(桑迪亚惯性地形辅助导航)将地形匹配融入EKF框架，其关键步骤包括：

状态向量：
x = [δN; δE; δψ; ε_N; ε_E]
包含水平位置误差、航向误差和INS速度误差
地形线性化：
h ≈ h₀ + ∇h_N·δN + ∇h_E·δE
其中∇h是地形梯度
观测方程：
z = h_INS - h_DEM = ∇h_N·δN + ∇h_E·δE + v

Matlab实现示例：

matlab复制function [x, P] = sitanUpdate(x, P, h_ins, h_dem, grad_n, grad_e, R)
    % 观测矩阵
    H = [grad_n, grad_e, 0, 0, 0];
    
    % 预测残差
    z_pred = H * x;
    innov = (h_ins - h_dem) - z_pred;
    
    % 卡尔曼增益
    S = H * P * H' + R;
    K = P * H' / S;
    
    % 状态更新
    x = x + K * innov;
    P = (eye(5) - K * H) * P;
end

地形导航实战经验：

地形梯度计算应采用5点或7点差分法，避免中心差分带来的平滑效应

在平坦区域应自动禁用地形更新，防止发散

结合粒子滤波可提高地形突变区域的鲁棒性

实时质量监测指标：新息序列应保持白噪声特性

6. MATLAB实现中的性能优化技巧

6.1 矩阵运算加速

在KF/EKF实现中，矩阵运算占用了大部分计算资源。通过以下优化，我曾将滤波速度提升3倍：

利用对称性：
- 协方差矩阵P始终对称，只需计算和下三角部分
- 使用(P + P')/2强制保持对称
稀疏矩阵：
- 当H矩阵稀疏时，使用稀疏存储格式
- 特别适用于多传感器融合场景
预分配内存：
- 在循环前预分配所有数组
- 避免动态增长数组

优化后的协方差更新示例：

matlab复制% 传统方式
P = (eye(n) - K * H) * P;

% 优化方式
PHt = P * H';
K = PHt / (H * PHt + R);
P = P - K * (H * P);
P = (P + P') / 2; % 保持对称

6.2 数值稳定性处理

KF/EKF中常见的数值问题及解决方案：

协方差矩阵失去正定性：
- 使用Joseph形式协方差更新：
  P = (I-KH)P(I-KH)' + KRK'
- 或采用平方根滤波（如Cholesky分解）
矩阵求逆不稳定：
- 添加正则化项：(HPH'+R + εI)⁻¹
- 使用伪逆或SVD分解
量测数据异常：
- 新息检测：‖z-Hx‖² > γ·trace(S)
- 鲁棒损失函数（如Huber函数）

平方根滤波的Matlab实现片段：

matlab复制function [x, S] = sqrtKalmanUpdate(x, S, z, H, R)
    % S是P的Cholesky分解: P = S*S'
    n = length(x);
    m = length(z);
    
    % 计算预测残差
    z_pred = H * x;
    innov = z - z_pred;
    
    % 构建复合矩阵
    A = [S' * H'; chol(R)];
    [Q, T] = qr(A);
    
    % 分割矩阵
    T11 = T(1:n,1:n);
    T21 = T(n+1:end,1:n);
    
    % 计算增益
    K = (S / T11') * T21';
    
    % 状态更新
    x = x + K * innov;
    
    % 协方差更新
    S = S / T11';
end

6.3 面向对象实现模式

对于复杂系统，我推荐采用面向对象的实现方式。以下是KF类的简化示例：

matlab复制classdef ExtendedKalmanFilter < handle
    properties
        x   % 状态估计
        P   % 协方差矩阵
        Q   % 过程噪声
        R   % 观测噪声
        f   % 状态转移函数
        h   % 观测函数
        F_jac % 状态转移雅可比
        H_jac % 观测雅可比
    end
    
    methods
        function obj = ExtendedKalmanFilter(init_x, init_P, f, h, F_jac, H_jac)
            obj.x = init_x;
            obj.P = init_P;
            obj.f = f;
            obj.h = h;
            obj.F_jac = F_jac;
            obj.H_jac = H_jac;
        end
        
        function predict(obj, u)
            % 预测步骤
            obj.x = obj.f(obj.x, u);
            F = obj.F_jac(obj.x, u);
            obj.P = F * obj.P * F' + obj.Q;
        end
        
        function update(obj, z)
            % 更新步骤
            H = obj.H_jac(obj.x);
            z_pred = obj.h(obj.x);
            innov = z - z_pred;
            
            S = H * obj.P * H' + obj.R;
            K = obj.P * H' / S;
            
            obj.x = obj.x + K * innov;
            obj.P = (eye(length(obj.x)) - K * H) * obj.P;
        end
    end
end

使用示例：

matlab复制% 定义模型函数
f = @(x,u) x + u;
h = @(x) x(1)^2 + x(2); % 非线性观测
F_jac = @(x,u) eye(2);
H_jac = @(x) [2*x(1), 1];

% 初始化EKF
ekf = ExtendedKalmanFilter([0;0], eye(2), f, h, F_jac, H_jac);
ekf.Q = 0.1*eye(2);
ekf.R = 1;

% 运行滤波
for i = 1:100
    ekf.predict([0.1; 0]);
    if mod(i,5) == 0
        z = measure(); % 获取观测
        ekf.update(z);
    end
end