容积卡尔曼滤波（CKF）仿真抛物线运动
容积卡尔曼滤波（Cubature Kalman Filter, CKF）的MATLAB实现。CKF是一种用于非线性系统状态估计的算法，它通过在状态空间中采样点（容积点）来近似非线性函数的高斯分布。
代码解析
参数设置
clc; clear all; close all;%% 参数设置
n = 51; % 状态序列长度
tf = 50; % 仿真总时刻数
dt = 0.1; % 时间步长
g = 9.81; % 重力加速度
首先，我们清除MATLAB环境，设置仿真参数，包括状态序列长度、仿真总时刻数、时间步长和重力加速度。
状态变量和观测初始化
%% 状态变量和观测初始化
x = zeros(4, n); % 系统状态
z = zeros(4, n); % 观测值
x(:, 1) = [0; 0; 10; 0]; % 初始状态 [水平位置; 垂直位置; 水平速度; 垂直速度]
x_ckf = zeros(4, n); % CKF滤波后的状态估计
x_ckf(:, 1) = [0; 0; 1; 0]; % CKF初始估计
xhat = x_ckf(:, 1);
这里初始化系统状态x和观测值z，以及CKF滤波后的状态估计x_ckf。初始状态设置为水平位置0，垂直位置0，水平速度10，垂直速度0。
噪声和协方差设置
%% 噪声和协方差设置
Q = diag([0.1, 0.1, 0.1, 0.1]); % 过程噪声协方差
R = diag([0.5, 0.5, 0.5, 0.5]); % 测量噪声协方差
P = eye(4) * 0.1; % 初始状态协方差
Pplus = P;
设置过程噪声协方差Q、测量噪声协方差R和初始状态协方差P。
容积点权重与分布
%% 容积点权重与分布
w = 0.25; % 每个采样点的权重
kesi = sqrt(2) * [1, 0, -1, 0; 0, 1, 0, -1; 1, -1, -1, 1; 0, -1, 0, 1];
定义容积点的权重w和分布kesi，这里使用的是2阶容积点。
状态方程与测量方程
%% 状态方程与测量方程
f = @(x) [x(1) + x(3) * dt; % 水平位置更新(抛物线运动)x(2) + x(4) * dt; % 垂直位置更新x(3); % 水平速度保持不变x(4) - g * dt]; % 垂直速度更新（加上重力影响）
h = @(x) [x(1); x(2); x(3); x(4)]; % 完整状态作为测量值
定义状态方程f和测量方程h，状态方程描述了抛物线运动的动力学模型，测量方程将完整状态作为测量值。
仿真系统
%% 仿真系统
for k = 1:tf% 真实状态更新x(:, k+1) = f(x(:, k)) + sqrt(Q) * randn(4, 1);% 观测值生成z(:, k+1) = h(x(:, k+1)) + sqrt(R) * randn(4, 1);
end
在仿真循环中，我们更新真实状态并生成观测值，同时添加过程噪声和测量噪声。
容积卡尔曼滤波
%% 容积卡尔曼滤波
for k = 1:tf%% Step 1: 生成容积采样点% Cholesky 分解tryS = chol(Pplus, 'lower');catchPplus = diag(max(diag(Pplus), epsilon)); % 使用对角替代S = chol(Pplus, 'lower');endrjpoint = S * kesi + repmat(xhat, 1, 4); % 当前状态的容积点%% Step 2: 容积点的状态传播Xminus = zeros(4, 4);for i = 1:4Xminus(:, i) = f(rjpoint(:, i));endxminus = w * sum(Xminus, 2); % 预测状态均值Pminus = w * ( ...(Xminus(:, 1) - xminus) * (Xminus(:, 1) - xminus)' + ...(Xminus(:, 2) - xminus) * (Xminus(:, 2) - xminus)' + ...(Xminus(:, 3) - xminus) * (Xminus(:, 3) - xminus)' + ...(Xminus(:, 4) - xminus) * (Xminus(:, 4) - xminus)' ...) + Q;%% Step 3: 测量预测Sminus = chol(Pminus, 'lower'); % 预测协方差的 Cholesky 分解rjpoint1 = Sminus * kesi + repmat(xminus, 1, 4); % 传播后的容积点Z = zeros(4, 4);for i = 1:4Z(:, i) = h(rjpoint1(:, i));endzhat = w * sum(Z, 2); % 预测测量均值Pzminus = w * ( ...(Z(:, 1) - zhat) * (Z(:, 1) - zhat)' + ...(Z(:, 2) - zhat) * (Z(:, 2) - zhat)' + ...(Z(:, 3) - zhat) * (Z(:, 3) - zhat)' + ...(Z(:, 4) - zhat) * (Z(:, 4) - zhat)' ...) + R; % 预测测量协方差% 交叉协方差Pxzminus = w * ( ...(rjpoint1(:, 1) - xminus) * (Z(:, 1) - zhat)' + ...(rjpoint1(:, 2) - xminus) * (Z(:, 2) - zhat)' + ...(rjpoint1(:, 3) - xminus) * (Z(:, 3) - zhat)' + ...(rjpoint1(:, 4) - xminus) * (Z(:, 4) - zhat)');%% Step 4: 更新K = Pxzminus / Pzminus; % 卡尔曼增益xhat = xminus + K * (z(:, k+1) - zhat); % 更新状态Pplus = Pminus - K * Pzminus * K'; % 更新协方差% 修复对称性Pplus = (Pplus + Pplus') / 2; % 特征值修复，确保正定性[V, D] = eig(Pplus);D(D < 1e-6) = 1e-6; % 调整所有特征值为小正值Pplus = V * D * V';% 强化数值稳定性epsilon = 1e-6; Pplus = Pplus + epsilon * eye(size(Pplus)); % 保存滤波后的状态x_ckf(:, k+1) = xhat;
end
在CKF循环中，我们执行以下步骤：
- 生成容积采样点。
- 容积点的状态传播。
- 测量预测。
- 更新状态和协方差。
完整代码
clc; clear all; close all;%% 参数设置
n = 51; % 状态序列长度
tf = 50; % 仿真总时刻数
dt = 0.1; % 时间步长
g = 9.81; % 重力加速度%% 状态变量和观测初始化
x = zeros(4, n); % 系统状态
z = zeros(4, n); % 观测值
x(:, 1) = [0; 0; 10; 0]; % 初始状态 [水平位置; 垂直位置; 水平速度; 垂直速度]
x_ckf = zeros(4, n); % CKF滤波后的状态估计
x_ckf(:, 1) = [0; 0; 1; 0]; % CKF初始估计
xhat = x_ckf(:, 1);%% 噪声和协方差设置
Q = diag([0.1, 0.1, 0.1, 0.1]); % 过程噪声协方差
R = diag([0.5, 0.5, 0.5, 0.5]); % 测量噪声协方差
P = eye(4) * 0.1; % 初始状态协方差
Pplus = P;%% 容积点权重与分布
w = 0.25; % 每个采样点的权重
kesi = sqrt(2) * [1, 0, -1, 0; 0, 1, 0, -1; 1, -1, -1, 1; 0, -1, 0, 1];%% 状态方程与测量方程
f = @(x) [x(1) + x(3) * dt; % 水平位置更新(抛物线运动)x(2) + x(4) * dt; % 垂直位置更新x(3); % 水平速度保持不变x(4) - g * dt]; % 垂直速度更新（加上重力影响）
h = @(x) [x(1); x(2); x(3); x(4)]; % 完整状态作为测量值%% 仿真系统
for k = 1:tf% 真实状态更新x(:, k+1) = f(x(:, k)) + sqrt(Q) * randn(4, 1);% 观测值生成z(:, k+1) = h(x(:, k+1)) + sqrt(R) * randn(4, 1);
end%% 容积卡尔曼滤波
for k = 1:tf%% Step 1: 生成容积采样点% Cholesky 分解tryS = chol(Pplus, 'lower');catchPplus = diag(max(diag(Pplus), epsilon)); % 使用对角替代S = chol(Pplus, 'lower');endrjpoint = S * kesi + repmat(xhat, 1, 4); % 当前状态的容积点%% Step 2: 容积点的状态传播Xminus = zeros(4, 4);for i = 1:4Xminus(:, i) = f(rjpoint(:, i));endxminus = w * sum(Xminus, 2); % 预测状态均值Pminus = w * ( ...(Xminus(:, 1) - xminus) * (Xminus(:, 1) - xminus)' + ...(Xminus(:, 2) - xminus) * (Xminus(:, 2) - xminus)' + ...(Xminus(:, 3) - xminus) * (Xminus(:, 3) - xminus)' + ...(Xminus(:, 4) - xminus) * (Xminus(:, 4) - xminus)' ...) + Q;%% Step 3: 测量预测Sminus = chol(Pminus, 'lower'); % 预测协方差的 Cholesky 分解rjpoint1 = Sminus * kesi + repmat(xminus, 1, 4); % 传播后的容积点Z = zeros(4, 4);for i = 1:4Z(:, i) = h(rjpoint1(:, i));endzhat = w * sum(Z, 2); % 预测测量均值Pzminus = w * ( ...(Z(:, 1) - zhat) * (Z(:, 1) - zhat)' + ...(Z(:, 2) - zhat) * (Z(:, 2) - zhat)' + ...(Z(:, 3) - zhat) * (Z(:, 3) - zhat)' + ...(Z(:, 4) - zhat) * (Z(:, 4) - zhat)' ...) + R; % 预测测量协方差% 交叉协方差Pxzminus = w * ( ...(rjpoint1(:, 1) - xminus) * (Z(:, 1) - zhat)' + ...(rjpoint1(:, 2) - xminus) * (Z(:, 2) - zhat)' + ...(rjpoint1(:, 3) - xminus) * (Z(:, 3) - zhat)' + ...(rjpoint1(:, 4) - xminus) * (Z(:, 4) - zhat)');%% Step 4: 更新K = Pxzminus / Pzminus; % 卡尔曼增益xhat = xminus + K * (z(:, k+1) - zhat); % 更新状态Pplus = Pminus - K * Pzminus * K'; % 更新协方差% 修复对称性Pplus = (Pplus + Pplus') / 2; % 特征值修复，确保正定性[V, D] = eig(Pplus);D(D < 1e-6) = 1e-6; % 调整所有特征值为小正值Pplus = V * D * V';% 强化数值稳定性epsilon = 1e-6; Pplus = Pplus + epsilon * eye(size(Pplus)); % 保存滤波后的状态x_ckf(:, k+1) = xhat;
end%% 绘图
state_labels = {'位置 x', '位置 y', '速度 v_x', '速度 v_y'};
for k = 1:4figure();plot(x(k, :), 'g-', 'LineWidth', 1); % 真实状态hold on;plot(x_ckf(k, :), 'b-', 'LineWidth', 1.5); % EKF 最优估计hold on;plot(z(k, :), 'k+', 'LineWidth', 1); % 状态测量值legend('真实状态', 'EKF 最优估计值', '状态测量值');xlabel('时间步');ylabel(state_labels{k});title(['状态 ', state_labels{k}]);set(gca, 'FontSize', 14);hold off;
end%% 全轨迹绘制
figure;
hold on;
plot(x(1, :), x(2, :), 'g-', 'LineWidth', 1.5); % 真实轨迹
plot(z(1, :), z(2, :), 'k+', 'MarkerSize', 6); % 测量轨迹
plot(x_ckf(1, :), x_ckf(2, :), 'b-', 'LineWidth', 1.5); % 估计轨迹
legend('真实轨迹', '测量轨迹', '估计轨迹');
xlabel('水平位置 x');
ylabel('垂直位置 y');
title('二维抛物线运动轨迹');
grid on;
axis equal;
hold off;
运行结果
