集合卡尔曼滤波（EnKF）的三维滤波（模拟平面定位）例程，带逐行注释

news2024/11/28 20:36:10

在这里插入图片描述

这段 MATLAB 代码实现了一个三维动态系统的集合卡尔曼滤波（Ensemble Kalman Filter, EnKF）示例。代码的主要目的是通过模拟真实状态和测量值，使用 EnKF 方法对动态系统状态进行估计。

文章目录

参数设置
初始化真实状态
定义状态转移和测量矩阵
生成真实状态和测量值
初始化集合卡尔曼滤波
执行集合卡尔曼滤波
绘制结果
误差绘图
总结
部分代码
运行结果

参数设置

状态维度 (n)：设置为6，表示系统有6个状态变量（如位置和速度）。
测量维度 (m)：设置为3，表示系统有3个测量变量。
时间步数 (N)：设置为100，表示模拟会进行100个时间步。
集合成员数量 (num_ensemble)：设置为10，表示使用10个样本进行状态估计。
过程噪声和测量噪声的协方差矩阵：
- process_noise_cov 设置为较小的值，表示过程噪声较低。
- measurement_noise_cov 设置为1，表示测量噪声较高。

初始化真实状态

创建一个 n 行 N 列的零矩阵 true_state 来存储真实状态。
设置初始真实状态为 [1; 0; 2; 1; 1; 0.1]，代表系统的初始位置和速度。

定义状态转移和测量矩阵

状态转移矩阵 (A)：定义如何从当前状态转移到下一个状态。这里使用了时间间隔 T 来更新位置和速度。
测量矩阵 (H)：定义如何从状态生成测量值，使用线性组合来映射状态到测量空间。

生成真实状态和测量值

在一个循环中，从第二个时间步开始：
- 使用状态转移矩阵 A 和过程噪声生成下一个真实状态。
- 根据真实状态和测量噪声生成测量值，存储在 measurements 矩阵中。

初始化集合卡尔曼滤波

生成初始集合 ensemble，所有成员都基于初始真实状态，加上随机扰动。
创建一个零矩阵 estimates 用于存储每个时间步的状态估计。

执行集合卡尔曼滤波

在循环中进行预测和更新步骤：
- 预测步骤：根据状态转移矩阵更新集合成员，加入过程噪声。
- 计算集合的均值和协方差：用于状态更新。
- 更新步骤：
  - 计算创新（观测值与预测值的差）。
  - 计算创新协方差。
  - 计算卡尔曼增益。
  - 更新集合成员，以反映新测量信息。
- 存储当前时间步的状态估计。

绘制结果

使用三维绘图（plot3）绘制真实状态、估计状态和测量值的轨迹。
创建多个子图，显示不同状态变量（如 X、Y、Z 位移）的真实状态、估计状态和测量值的变化。

误差绘图

新建图形窗口，绘制估计误差和观测误差。
使用 fprintf 输出 X、Y、Z 轴的误差统计信息，包括最大误差和平均绝对误差。

总结

整段代码的主要目标是通过集合卡尔曼滤波算法对一个三维动态系统进行状态估计。它通过模拟真实状态和测量值的过程，展示了如何使用 EnKF 来处理动态系统，并通过可视化结果和误差分析来评估估计的准确性。

部分代码

% 集合卡尔曼滤波示例 三维
% 2024-11-12/Ver1
clear; clc; close all; % 清除工作空间，清空命令窗口，关闭所有图形窗口
rng(0); % 设置随机数生成器的种子，以确保结果可重复

% 参数设置
n = 6; % 状态维度（4个状态变量）
m = 3; % 测量维度（2个测量变量）
N = 100; % 时间步数（总共进行100个时间步的模拟）
num_ensemble = 10; % 集合成员数量（使用10个样本进行估计）
process_noise_cov = 1e-5 * eye(n); % 过程噪声协方差矩阵（小值，表示低噪声）
measurement_noise_cov = 1 * eye(m); % 测量噪声协方差矩阵（较大值，表示较高噪声）

% 初始化真实状态
true_state = zeros(n, N); % 创建一个n行N列的零矩阵，用于存储真实状态
true_state(:, 1) = [1; 0; 2; 1; 1 ; 0.1]; % 设置初始真实状态（X位移、X速度、Y位移、Y速度）

T = 1; %时间间隔
% 状态转移矩阵
A = [1 T 0 0 0 0;  % 状态转移矩阵，定义如何从一个状态转移到下一个状态
     0 1 0 0 0 0 ; 
     0 0 1 T 0 0; 
     0 0 0 1 0 0;
     0 0 0 0 1 T;
     0 0 0 0 0 1];

% 测量矩阵
H = [1 0 0 0 0 0; % 测量矩阵，定义如何从状态生成测量值
     0 0 1 0 0 0;
     0 0 0 0 1 0];