【状态估计】基于二阶扩展卡尔曼滤波（Second-Order EKF, SO-EKF）实现 “质量 - 弹簧 - 阻尼（MSD）系统”状态估计附matlab代码
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
一、背景
（一）MSD 系统的重要性与应用
“质量 - 弹簧 - 阻尼（MSD）系统” 是一种广泛应用于多个领域的基础动力学模型。在机械工程中，常用于分析振动系统，如汽车的悬挂系统、建筑物的抗震结构等。在生物医学工程里，可模拟人体关节的运动力学。该系统通过质量块、弹簧和阻尼器的相互作用，描述了物体在力的作用下的动态响应，理解和准确估计其状态对于优化系统性能、保障系统安全至关重要。
（二）状态估计的必要性
在实际应用中，MSD 系统的状态（如质量块的位置、速度和加速度）很难直接精确测量。然而，这些状态信息对于系统的控制、监测和故障诊断至关重要。例如，在汽车悬挂系统中，准确知道质量块（车身部分）的位置和速度，有助于实时调整减震器的阻尼，提高行驶舒适性和稳定性。因此，需要通过状态估计方法，根据可测量的输出（如传感器获取的部分物理量）来推断系统的内部状态。
（三）传统卡尔曼滤波与扩展卡尔曼滤波的局限
传统卡尔曼滤波（KF）适用于线性系统且噪声服从高斯分布的情况。但 MSD 系统本质上是非线性的，其动力学方程具有非线性特性（如弹簧力与位移的非线性关系），传统 KF 无法直接应用。扩展卡尔曼滤波（EKF）通过对非线性函数进行一阶泰勒展开，将非线性系统近似为线性系统，从而应用卡尔曼滤波框架。然而，EKF 仅考虑一阶近似，对于强非线性系统，这种近似可能引入较大误差，导致估计精度下降。
（二）SO - EKF 应用于 MSD 系统
对于 MSD 系统的非线性状态转移函数和测量函数，在 SO - EKF 的预测和更新步骤中，按照二阶泰勒展开的方式进行处理。通过计算雅可比矩阵和海森矩阵，将二阶项纳入状态预测和更新的计算中。例如，在预测步骤中，利用二阶展开的状态转移函数计算预测状态，更准确地考虑系统的非线性动态，相比 EKF 能更好地跟踪质量块的真实运动状态。
在更新步骤中，基于二阶展开的测量函数计算测量预测和卡尔曼增益，使测量值对状态估计的修正更加精确。通过不断迭代预测和更新步骤，SO - EKF 能够逐步提高对 MSD 系统状态（位移、速度和加速度）的估计精度，为 MSD 系统的控制、监测和优化提供可靠的状态信息。
综上所述，基于二阶扩展卡尔曼滤波实现 “质量 - 弹簧 - 阻尼系统” 状态估计，利用 SO - EKF 对非线性函数更精确的近似能力，有效提高了在非线性 MSD 系统中的状态估计精度，相比传统 EKF 更能满足实际应用中对系统状态准确估计的需求。
⛳️ 运行结果
📣 部分代码
; clear; clc;
%% Params
m = 5; % [kg], mass
b = 0.5; % [N*s/m], damping coefficient
Vmag = 0.05; % [m^2/s], noise magnitude
Kval = 1.5; % [N/m], actual stiffness (to be estimated)
%% Equations
% States: X = [x1; x2; x3] = [position; velocity; spring constant]
% Input: U = F = Force input
% Meas: Y = [x1; Fstiffness] (to make it nonlinear, otherwise Hxx would be 0 matrix)
syms t x1 x2 x3 U
X = [x1; x2; x3];
xdot = [x2; -x3*x1/m - b*x2/m - U/m; 0];
fx = jacobian(xdot,X)*X;
gx = jacobian(xdot,U)*U;
hx = [x1; x3*x1/m];