Simulink中trackingKF滤波器的更新与预测实现问题咨询

问题背景 #

我想在Simulink中创建一个跟踪滤波器，示例初始化代码如下：

filter = trackingKF('MotionModel', '2D Constant Acceleration', 'State', zeros(6, 1));

之后希望在仿真过程中接收位置反馈，更新滤波器并预测状态，定义的函数如下：

function predictedState = Predict(x,y)
    
    % 接收当前位置反馈
    presentPosition = [x;y]; 
    
    % 校正步骤：用观测位置更新滤波器
    correct(filter, presentPosition);
    
    % 接收反馈后预测未来位置
    predictedState = predict(filter);
 
end

我遇到的问题是：如何在Simulink中定义可通过correct命令更新的滤波器，以及如何搭建仿真结构，用上述函数在仿真过程中持续更新该滤波器？

我曾尝试在Predict函数内部定义滤波器，但效果很差——因为滤波器需要持续保留状态才能提升精度。Model Explorer中的回调函数能否解决这个问题？

解决方案 #

1. 用Persistent变量保存滤波器状态（推荐） #

在Predict函数中把filter定义为persistent变量，让它的状态在多次函数调用之间持续保留，避免每次调用都重新初始化：

function predictedState = Predict(x,y)
    persistent filter  % 持久化变量，跨调用保留滤波器状态
    
    % 首次调用时初始化滤波器
    if isempty(filter)
        filter = trackingKF('MotionModel', '2D Constant Acceleration', 'State', zeros(6, 1));
    end
    
    % 接收当前位置反馈
    presentPosition = [x;y]; 
    
    % 用观测位置校正滤波器
    correct(filter, presentPosition);
    
    % 预测状态
    predictedState = predict(filter);
end

在Simulink中添加MATLAB Function模块，导入该函数，将模块输入设为x和y，输出设为predictedState即可。仿真过程中，每次模块被调用，filter的状态都会持续更新。

2. 用Simulink数据存储内存模块保存滤波器 #

如果不想依赖MATLAB Function的持久化变量，可以用Simulink原生模块实现状态保留：

• 在Model Explorer中创建Simulink.Parameter对象，类型设为trackingKF，初始化值设为：trackingKF('MotionModel', '2D Constant Acceleration', 'State', zeros(6, 1))
• 添加Data Store Memory模块，关联上述参数作为存储变量
• 在MATLAB Function模块中声明外部调用函数，读取存储的滤波器、执行校正预测后写回：

function predictedState = Predict(x,y)
    coder.extrinsic('correct', 'predict', 'assignin', 'evalin');
    % 从基础工作区读取滤波器
    filter = evalin('base', 'filter');  
    
    presentPosition = [x;y];
    correct(filter, presentPosition);
    predictedState = predict(filter);
    
    % 将更新后的滤波器写回工作区
    assignin('base', 'filter', filter);  
end

注意需确保仿真前已完成滤波器的初始化。

3. Model Explorer回调函数的作用 #

回调函数仅适合做一次性初始化，比如在模型启动时（InitFcn回调）初始化滤波器：

• 打开Model Explorer，找到模型的InitFcn回调，添加代码：

filter = trackingKF('MotionModel', '2D Constant Acceleration', 'State', zeros(6, 1));

但回调函数无法在仿真过程中持续更新状态，必须配合前述持久化变量或数据存储方案才能实现滤波器的连续状态更新。

内容的提问来源于stack exchange，提问作者Minh Duc Pham