引言：本文以Model-based observers for vehicle dynamics and tyre force prediction这篇论文为例，讲解EKF的设计流程（见下文），最后应用到simulink上，保证能生成的C代码，应用到嵌入式系统中。并以此为范例，为以后的EKF估计算法提供模板。

1.流程图

2.连续状态和观测空间方程

考虑车辆横向动力学系统，其连续时间非线性状态空间模型可表示为

由于状态方程和观测方程均为非线性形式，因此需采用 EKF 进行状态估计。

3.离散化

离散化方法有一阶欧拉法、拉氏变换、向前欧拉、向后欧拉、牛顿梯形、改进欧拉法等，这里仅需要掌握一阶欧拉即可，其满足条件需要离散步长较小，这里我选0.005s为离散步长。

（1）一阶欧拉法

则

（2）拉氏变换

（3）向前欧拉、向后欧拉、牛顿梯形、改进欧拉法

见数值计算方程的常见微分方程积分

4.可观性判据

EKF 能否有效估计系统状态的前提是系统在所选状态和观测配置下具备可观性。
对于非线性系统，通常在工作点处进行线性化，构造可观性矩阵

若 rank(V)=n，满秩

则系统在该工作点附近是局部可观的，其中 n为状态维数。

5.雅克比矩阵计算

EKF 的核心思想是在当前估计点对非线性函数进行一阶泰勒展开，因此需要计算状态方程和观测方程的雅可比矩阵。

6.迭代方程

function [x_hat, P] = ekf_tire_force_step( ...
    G, Dpk, x_hat, G_YKB, P, Q, H, H_YKB, Dmk, R, y)
%--------------------------------------------------------------------------
% 扩展卡尔曼滤波（EKF）单步递推函数 —— 轮胎力估计
%
% 输入参数：
% G      : 离散状态转移矩阵
% Dp     : 状态方程中的常数项（如滚阻等）
% x_hat  : 上一时刻状态估计值 x̂(k|k)   (6×1)
% G_YKB  : 状态方程对状态的雅可比矩阵 ∂f/∂x
% P      : 上一时刻状态协方差矩阵 P(k|k) (6×6)
% Q      : 过程噪声协方差矩阵
% H      : 观测矩阵
% H_YKB  : 观测方程对状态的雅可比矩阵 ∂h/∂x
% Dmk    : 观测方程中的常数项
% R      : 测量噪声协方差矩阵
% y      : 当前时刻测量值 [ax ay r vx]' (4×1)
%
% 输出参数：
% x_hat  : 当前时刻状态估计值 x̂(k|k)
% P      : 当前时刻状态协方差矩阵 P(k|k)
%--------------------------------------------------------------------------

%% ==================== 预测阶段（Time Update） ====================

% 状态预测：根据系统状态方程预测下一时刻状态
% x̂(k|k−1) = f(x̂(k−1|k−1))
x_pred = G * x_hat + Dpk;

% 协方差预测：
% P(k|k−1) = F(k)·P(k−1|k−1)·Fᵀ(k) + Q
% 其中 F(k) 为状态方程的雅可比矩阵
P_pred = G_YKB * P * G_YKB' + Q;

% 观测预测：
% ŷ(k|k−1) = h(x̂(k|k−1))
y_pred = H * x_pred + Dmk;

%% ==================== 更新阶段（Measurement Update） ====================

% 卡尔曼增益计算：
% K(k) = P(k|k−1)·Hᵀ(k)·[H(k)·P(k|k−1)·Hᵀ(k) + R]⁻¹
K = P_pred * H_YKB' / (H_YKB * P_pred * H_YKB' + R);

% 状态更新：
% x̂(k|k) = x̂(k|k−1) + K(k)·[y(k) − ŷ(k|k−1)]
x_hat = x_pred + K * (y - y_pred);

% 协方差更新：
% P(k|k) = [I − K(k)·H(k)]·P(k|k−1)
P = (eye(6) - K * H_YKB) * P_pred;

end

7.模块框架