1. 卡尔曼滤波与状态空间模型基础
在工程实践中，我们经常需要从含有噪声的观测数据中估计系统的真实状态。卡尔曼滤波（Kalman Filter）正是解决这类问题的经典算法。它通过递归方式，结合系统模型和观测数据，实现对系统状态的最优估计。
1.1 状态空间方程表示
状态空间模型是现代控制理论中描述动态系统的标准形式。对于一个离散时间系统，其状态空间方程通常表示为：
code复制x_k = F_k * x_{k-1} + B_k * u_k + w_k
z_k = H_k * x_k + v_k
其中：
- x_k 是k时刻的系统状态向量
- F_k 是状态转移矩阵
- B_k 是控制输入矩阵
- u_k 是控制输入向量
- w_k 是过程噪声（通常假设为高斯白噪声）
- z_k 是观测向量
- H_k 是观测矩阵
- v_k 是观测噪声（通常假设为高斯白噪声）
在MATLAB中，我们可以使用ss函数创建状态空间模型：
matlab复制sys = ss(F, B, H, D, Ts)
其中Ts是采样时间，D是直接传递矩阵。
1.2 卡尔曼滤波基本原理
卡尔曼滤波通过两个主要步骤实现状态估计：
-
预测步骤：
- 状态预测：x̂_k|k-1 = F_k * x̂_k-1|k-1 + B_k * u_k
- 协方差预测：P_k|k-1 = F_k * P_k-1|k-1 * F_k' + Q_k
-
更新步骤：
- 卡尔曼增益计算：K_k = P_k|k-1 * H_k' * (H_k * P_k|k-1 * H_k' + R_k)^-1
- 状态更新：x̂_k|k = x̂_k|k-1 + K_k * (z_k - H_k * x̂_k|k-1)
- 协方差更新：P_k|k = (I - K_k * H_k) * P_k|k-1
其中Q_k和R_k分别是过程噪声和观测噪声的协方差矩阵。
2. 扩展卡尔曼滤波(EKF)实现
2.1 EKF算法原理
当系统为非线性时，标准的卡尔曼滤波不再适用。EKF通过局部线性化的方式处理非线性系统。对于非线性系统：
code复制x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k
EKF的实现步骤如下：
-
预测步骤：
- 状态预测：x̂_k|k-1 = f(x̂_k-1|k-1, u_k)
- 协方差预测：P_k|k-1 = F_{k-1} * P_k-1|k-1 * F_{k-1}' + Q_k
- 其中F_{k-1}是f在x̂_k-1|k-1处的雅可比矩阵
-
更新步骤：
- 计算观测雅可比矩阵H_k
- 卡尔曼增益：K_k = P_k|k-1 * H_k' * (H_k * P_k|k-1 * H_k' + R_k)^-1
- 状态更新：x̂_k|k = x̂_k|k-1 + K_k * (z_k - h(x̂_k|k-1))
- 协方差更新：P_k|k = (I - K_k * H_k) * P_k|k-1
2.2 MATLAB实现要点
在MATLAB中实现EKF时，需要注意以下关键点：
- 雅可比矩阵计算：
matlab复制% 定义符号变量
syms x1 x2 x3 ... xn
x = [x1; x2; ... ; xn];
% 计算状态转移函数的雅可比
F = jacobian(f(x,u), x);
% 计算观测函数的雅可比
H = jacobian(h(x), x);
- 噪声协方差矩阵设置：
matlab复制Q = diag([q1, q2, ..., qn]); % 过程噪声协方差
R = diag([r1, r2, ..., rm]); % 观测噪声协方差
- 初始化：
matlab复制x_hat = x0; % 初始状态估计
P = P0; % 初始协方差矩阵
3. 无迹卡尔曼滤波(UKF)实现
3.1 UKF算法原理
UKF采用无迹变换(Unscented Transform)来处理非线性问题，相比EKF的线性化近似，UKF能够更准确地捕捉非线性特性的统计特性。UKF的核心步骤如下：
-
Sigma点生成：
- 选择2n+1个Sigma点（n为状态维数）
- χ_{k-1} = [x̂_{k-1}, x̂_{k-1}±√((n+λ)P_{k-1})]
-
预测步骤：
- 传播Sigma点：χ_k|k-1 = f(χ_{k-1}, u_k)
- 计算预测状态和协方差：
- x̂_k|k-1 = Σ W_i^m χ_
- P_k|k-1 = Σ W_i^c (χ_{i,k|k-1}-x̂_k|k-1)(χ_{i,k|k-1}-x̂_k|k-1)' + Q_k
-
更新步骤：
- 传播观测Sigma点：Z_k|k-1 = h(χ_k|k-1)
- 计算预测观测和协方差：
- ẑ_k|k-1 = Σ W_i^m Z_
- P_{z,z} = Σ W_i^c (Z_{i,k|k-1}-ẑ_k|k-1)(Z_{i,k|k-1}-ẑ_k|k-1)' + R_k
- P_{x,z} = Σ W_i^c (χ_{i,k|k-1}-x̂_k|k-1)(Z_{i,k|k-1}-ẑ_k|k-1)'
- 计算卡尔曼增益和更新状态：
- K_k = P_{x,z} * P_{z,z}^-1
- x̂_k|k = x̂_k|k-1 + K_k * (z_k - ẑ_k|k-1)
- P_k|k = P_k|k-1 - K_k * P_{z,z} * K_k'
3.2 MATLAB实现技巧
- Sigma点生成函数：
matlab复制function X = sigmaPoints(x, P, lambda)
n = length(x);
X = zeros(n, 2*n+1);
X(:,1) = x;
sqrtP = chol((n+lambda)*P)';
for i = 1:n
X(:,i+1) = x + sqrtP(:,i);
X(:,i+n+1) = x - sqrtP(:,i);
end
end
- 权重计算：
matlab复制alpha = 1e-3; % 控制Sigma点分布
kappa = 0; % 二阶参数
beta = 2; % 包含先验分布信息
lambda = alpha^2*(n+kappa) - n;
Wm = [lambda/(n+lambda), 0.5/(n+lambda)*ones(1,2*n)];
Wc = [(lambda/(n+lambda)+(1-alpha^2+beta)), 0.5/(n+lambda)*ones(1,2*n)];
- 预测更新循环：
matlab复制for k = 1:N
% 生成Sigma点
X = sigmaPoints(x_hat, P, lambda);
% 预测步骤
X_pred = f(X, u(:,k));
x_pred = X_pred * Wm';
P_pred = zeros(n);
for i = 1:2*n+1
P_pred = P_pred + Wc(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)';
end
P_pred = P_pred + Q;
% 更新步骤
Z_pred = h(X_pred);
z_pred = Z_pred * Wm';
Pzz = zeros(m);
Pxz = zeros(n,m);
for i = 1:2*n+1
Pzz = Pzz + Wc(i)*(Z_pred(:,i)-z_pred)*(Z_pred(:,i)-z_pred)';
Pxz = Pxz + Wc(i)*(X_pred(:,i)-x_pred)*(Z_pred(:,i)-z_pred)';
end
Pzz = Pzz + R;
K = Pxz / Pzz;
x_hat = x_pred + K*(z(:,k) - z_pred);
P = P_pred - K*Pzz*K';
end
4. 9维状态空间实现案例
4.1 系统建模
考虑一个9维状态空间的导航系统，状态向量包含：
code复制x = [px, py, pz, vx, vy, vz, ax, ay, az]'
即位置、速度和加速度在三个坐标轴上的分量。
状态转移函数（假设加速度变化为随机游走）：
code复制f(x,u) = [px + vx*dt + 0.5*ax*dt^2
py + vy*dt + 0.5*ay*dt^2
pz + vz*dt + 0.5*az*dt^2
vx + ax*dt
vy + ay*dt
vz + az*dt
ax + wa_x
ay + wa_y
az + wa_z]
观测函数（假设直接观测位置）：
code复制h(x) = [px, py, pz]'
4.2 MATLAB实现框架
- 初始化参数：
matlab复制dt = 0.1; % 采样时间
n = 9; % 状态维度
m = 3; % 观测维度
% 过程噪声协方差
Q = diag([0.01, 0.01, 0.01, 0.1, 0.1, 0.1, 1, 1, 1]);
% 观测噪声协方差
R = diag([0.1, 0.1, 0.1]);
% 初始状态和协方差
x_true = zeros(n,1);
x_hat = x_true + randn(n,1)*0.5;
P = eye(n);
- EKF实现核心：
matlab复制% 定义符号变量计算雅可比矩阵
syms px py pz vx vy vz ax ay az
x_sym = [px; py; pz; vx; vy; vz; ax; ay; az];
% 状态转移函数
f = [px + vx*dt + 0.5*ax*dt^2;
py + vy*dt + 0.5*ay*dt^2;
pz + vz*dt + 0.5*az*dt^2;
vx + ax*dt;
vy + ay*dt;
vz + az*dt;
ax;
ay;
az];
% 观测函数
h = [px; py; pz];
% 计算雅可比矩阵
F_jac = jacobian(f, x_sym);
H_jac = jacobian(h, x_sym);
% 转换为MATLAB函数
F_func = matlabFunction(F_jac, 'Vars', {x_sym});
H_func = matlabFunction(H_jac, 'Vars', {x_sym});
% EKF主循环
for k = 1:N
% 预测步骤
x_pred = [x_hat(1) + x_hat(4)*dt + 0.5*x_hat(7)*dt^2;
x_hat(2) + x_hat(5)*dt + 0.5*x_hat(8)*dt^2;
x_hat(3) + x_hat(6)*dt + 0.5*x_hat(9)*dt^2;
x_hat(4) + x_hat(7)*dt;
x_hat(5) + x_hat(8)*dt;
x_hat(6) + x_hat(9)*dt;
x_hat(7);
x_hat(8);
x_hat(9)];
F = F_func(x_hat);
P_pred = F * P * F' + Q;
% 更新步骤
H = H_func(x_pred);
K = P_pred * H' / (H * P_pred * H' + R);
z_true = [x_true(1); x_true(2); x_true(3)] + sqrt(R)*randn(m,1);
z_pred = [x_pred(1); x_pred(2); x_pred(3)];
x_hat = x_pred + K * (z_true - z_pred);
P = (eye(n) - K*H) * P_pred;
end
- UKF实现补充：
matlab复制% UKF参数
alpha = 1e-3;
kappa = 0;
beta = 2;
lambda = alpha^2*(n+kappa) - n;
% 权重计算
Wm = [lambda/(n+lambda), 0.5/(n+lambda)*ones(1,2*n)];
Wc = [(lambda/(n+lambda)+(1-alpha^2+beta)), 0.5/(n+lambda)*ones(1,2*n)];
% UKF主循环
for k = 1:N
% Sigma点生成
X = sigmaPoints(x_hat, P, lambda);
% 预测步骤
X_pred = zeros(n, 2*n+1);
for i = 1:2*n+1
X_pred(:,i) = [X(1,i) + X(4,i)*dt + 0.5*X(7,i)*dt^2;
X(2,i) + X(5,i)*dt + 0.5*X(8,i)*dt^2;
X(3,i) + X(6,i)*dt + 0.5*X(9,i)*dt^2;
X(4,i) + X(7,i)*dt;
X(5,i) + X(8,i)*dt;
X(6,i) + X(9,i)*dt;
X(7,i);
X(8,i);
X(9,i)];
end
x_pred = X_pred * Wm';
P_pred = zeros(n);
for i = 1:2*n+1
P_pred = P_pred + Wc(i)*(X_pred(:,i)-x_pred)*(X_pred(:,i)-x_pred)';
end
P_pred = P_pred + Q;
% 更新步骤
Z_pred = X_pred(1:3,:);
z_pred = Z_pred * Wm';
Pzz = zeros(m);
Pxz = zeros(n,m);
for i = 1:2*n+1
Pzz = Pzz + Wc(i)*(Z_pred(:,i)-z_pred)*(Z_pred(:,i)-z_pred)';
Pxz = Pxz + Wc(i)*(X_pred(:,i)-x_pred)*(Z_pred(:,i)-z_pred)';
end
Pzz = Pzz + R;
K = Pxz / Pzz;
x_hat = x_pred + K*(z_true - z_pred);
P = P_pred - K*Pzz*K';
end
4.3 性能评估与比较
在实际应用中，EKF和UKF各有优劣：
-
计算复杂度：
- EKF：O(n^3)，主要来自矩阵求逆和乘法
- UKF：O(n^3)，但需要计算2n+1个Sigma点
-
精度比较：
- 对于弱非线性系统，EKF和UKF性能相近
- 对于强非线性系统，UKF通常表现更好
-
实现难度：
- EKF需要推导雅可比矩阵，对复杂系统可能困难
- UKF无需雅可比矩阵，但需要合理选择Sigma点参数
-
鲁棒性：
- UKF对初始误差和模型不匹配更具鲁棒性
- EKF在参数选择不当时容易发散
在实际工程中，选择哪种滤波器取决于具体应用场景、系统非线性程度和计算资源限制。对于9维状态空间这样的中等维度问题，两种方法在普通计算机上都能实时运行。