Simulink 中建立微分方程

近日使用 Simulink-Carsim 联合仿真做横摆力矩控制，因为用到了一个滑模观测器对车辆侧偏角进行观测，需要在 simulink 中搭建一个微分方程模块，长期使用脚本编程，不太习惯复杂冗长的 s-function，于是使用 matlab function 搭建微分方程模块。
首先给出滑模观测器（Sliding Mode Observer SMO）的表达式：
$$\{\begin{array}{l}\dot{\hat{\beta }}=A_{11}\hat{\beta }+A_{12}\hat{\gamma }+B_{1}u+k_1{k}_2\mathrm{sign}(\tilde{\gamma })+k_3{\tilde{a}}_y\\ \dot{\hat{\gamma }}=A_{21}\hat{\beta }+A_{22}\hat{\gamma }+B_{2}u+k_1\mathrm{sign}(\tilde{\gamma })+k_4{\tilde{a}}_y\\ {\hat{a}}_y=C_{21}\hat{\beta }+C_{22}\hat{\gamma }+E_{2}u\end{array}$$

公式具体系数按下不表，读者可以参考 Active Front Steering-based Electronic Stability Control for Steer-by-Wire Vehicles via Terminal Sliding Mode and Extreme Learning Machine。
simulink 设置为 ode3 求解器，固定步长 $0.001s$，搭建 simulink 模块如下图所示：

主要的思路是使用一个 Memory 模块来记录系统变量前一时刻值，需要注意的是，每个 Memory 都需要填入初始值后使用。

具体函数 matlab function 定义如下：

function [betahat,rhat,ayhat]   = fcn(beta,deltaf,r,ay,betahat,rhat,ayhat)
% This is the function of SMO
% Xhat    the inner variable for SMO
% ay      the lateral acceleration
% r       the yaw rate γ
% beta    the vehicle slideslip angle
% deltaf   the driver's steer command
% ayhat   the observed value for ay
% rhat the observerd value for γ
% betahat the observerd value for β

% hu 2020-11-13
% hu 2020-11-14 tested a set of parameters

%--- Vehicle contants give here ---

lf = 1.016;
lr = 1.562;
Cf = 57000; 
Cr = 68000;
m  = 1274;
Vx = 15;
Iz = 1523;

A11 = -2 * (Cf + Cr) / (m * Vx);
A12 = 2 * (Cr * lr - Cf * lf) / (m * Vx^2) - 1;
A21 = 2 * (Cr * lr - Cf * lf) / Iz;
A22 = -2 * (lf^2 * Cf + lr^2 * Cr) / (Iz * Vx);
B1  = 2 * Cf / (m * Vx);
B2  = 2 * lf * Cf / Iz;
C21 = Vx * A11;
C22 = Vx * (A12 + 1);
E2  = Vx * B1;
h   = .001;

k1 = 7; 
k2 = .4; 
k3 = .3; 
k4 = 10;

delta = .1; % boundary layer .3
sat   = @(s)(s >= delta) * sign(s) + (s < delta) * (s / delta);  % sat function def.

u       = deltaf;
e       = [beta - betahat;r - rhat;ay - ayhat];

%--- Main loop ---
betahatdot = A11 * betahat + A12 * rhat + B1 * u + k1 * k2 * sat(e(2)) + k3 * e(3);
betahat    = betahat + betahatdot * h;
rhatdot    = A21 * betahat + A22 * rhat + B2 * u + k1 * sat(e(2)) + k4 * e(3);
rhat       = rhat + rhatdot * h;
ayhat      = C21 * betahat + C22 * rhat + E2 * u;