容积卡尔曼滤波（CKF）仿真抛物线运动

Ning_.2024-12-07 16:51

容积卡尔曼滤波（CKF）仿真抛物线运动

容积卡尔曼滤波（Cubature Kalman Filter, CKF）的MATLAB实现。CKF是一种用于非线性系统状态估计的算法，它通过在状态空间中采样点（容积点）来近似非线性函数的高斯分布。

代码解析

参数设置

matlab 复制代码 
clc; clear all; close all;

%% 参数设置
n = 51;                % 状态序列长度
tf = 50;               % 仿真总时刻数
dt = 0.1;              % 时间步长
g = 9.81;              % 重力加速度

首先，我们清除MATLAB环境，设置仿真参数，包括状态序列长度、仿真总时刻数、时间步长和重力加速度。

状态变量和观测初始化

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。

噪声和协方差设置

matlab 复制代码 
%% 噪声和协方差设置
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。

容积点权重与分布

matlab 复制代码 
%% 容积点权重与分布
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阶容积点。

状态方程与测量方程

matlab 复制代码 
%% 状态方程与测量方程
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，状态方程描述了抛物线运动的动力学模型，测量方程将完整状态作为测量值。

仿真系统

matlab 复制代码 
%% 仿真系统
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

在仿真循环中，我们更新真实状态并生成观测值，同时添加过程噪声和测量噪声。

容积卡尔曼滤波

matlab 复制代码 
%% 容积卡尔曼滤波
for k = 1:tf
    %% Step 1: 生成容积采样点
    % Cholesky 分解
    try
        S = chol(Pplus, 'lower');
    catch
        Pplus = diag(max(diag(Pplus), epsilon)); % 使用对角替代
        S = chol(Pplus, 'lower');
    end
    
    rjpoint = S * kesi + repmat(xhat, 1, 4);     % 当前状态的容积点

    %% Step 2: 容积点的状态传播
    Xminus = zeros(4, 4);
    for i = 1:4
        Xminus(:, i) = f(rjpoint(:, i));
    end
    
    xminus = 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:4
        Z(:, i) = h(rjpoint1(:, i));
    end
    zhat = 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循环中，我们执行以下步骤：

  1. 生成容积采样点。
  2. 容积点的状态传播。
  3. 测量预测。
  4. 更新状态和协方差。

完整代码

matlab 复制代码 
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 分解
    try
        S = chol(Pplus, 'lower');
    catch
        Pplus = diag(max(diag(Pplus), epsilon)); % 使用对角替代
        S = chol(Pplus, 'lower');
    end
    
    rjpoint = S * kesi + repmat(xhat, 1, 4);     % 当前状态的容积点

    %% Step 2: 容积点的状态传播
    Xminus = zeros(4, 4);
    for i = 1:4
        Xminus(:, i) = f(rjpoint(:, i));
    end
    
    xminus = 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:4
        Z(:, i) = h(rjpoint1(:, i));
    end
    zhat = 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:4
  figure();
  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;

运行结果

相关推荐
hn小菜鸡42 分钟前
LeetCode 2320.统计放置房子的方式数
算法·leetcode·职场和发展
高一学习c++会秃头吗43 分钟前
leetcode_547 省份数量
算法·leetcode·职场和发展
天赐细莲1 小时前
(仓颉) Cangjie 刷力扣基础语法小结
数据结构·算法·leetcode·职场和发展
dundunmm1 小时前
论文阅读：Statistical Comparisons of Classifiers over Multiple Data Sets
论文阅读·人工智能·算法·机器学习·评估方法
pzx_0011 小时前
【论文阅读】相似误差订正方法在风电短期风速预报中的应用研究
开发语言·论文阅读·python·算法·leetcode·sklearn
小雄abc2 小时前
决定系数R2 浅谈三 : 决定系数R2与相关系数r的关系、决定系数R2是否等于相关系数r的平方
经验分享·笔记·深度学习·算法·机器学习·学习方法·论文笔记
uyeonashi2 小时前
【C++】刷题强训(day14)--乒乓球匡、组队竞赛、删除相邻数字的最大分数
开发语言·c++·算法·哈希算法
机器学习之心3 小时前
一区正弦余弦算法！SCA-SVM正弦余弦算法优化支持向量机多特征分类预测
算法·支持向量机·分类·sca-svm·正弦余弦算法优化
a栋栋栋4 小时前
刷算法心得
算法
華華3554 小时前
读程序题...
开发语言·c++·算法
热门推荐
01(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明02【halcon】halcon字符识别——OCR03玄机平台应急响应—webshell查杀04Windows11 Copilot助手开启教程(免费GPT-4)0504 - matlab m_map地学绘图工具基础函数 - 设置网格06Docker 夺命连环 15 问07【一文读懂】NTN（非地面网络）技术介绍08支付宝短视频平台创作分成激励项目09PyTorch机器学习实现液态神经网络10『手写ahooks系列-useClickAway』一个hook解决监听点击元素外部