国家你想使用不符合您所使用的模型状态转换(constvel)和测量(cvmeas)。这两种模式会有一个三维恒定速度状态[x, vx; y; v; z; vz]和cvmeas提供了一个测量,三维位置测量[x, y, z],使用默认MeasurementParameters价值。
如果您想要使用的“trackingEKF”功能模型如上所述,您必须编写自己的状态转换函数,测量功能,相应的雅可比矩阵函数。这里有一些初步的建议这四个功能的代码:
%状态转换函数:
函数国家= constvel2d(状态,dt,变长度输入宗量)
%验证状态长4个元素,有两个输入
A1d = [1 dt;0 1];
= blkdiag (A1d A1d);
状态(,)= *状态(:);
结束
%测量功能:
函数变长度输入宗量量= cvmeas2DFullState(状态)
%长4个元素验证状态
H = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1);
量= H *国家(:);
结束
%状态转换雅可比矩阵:
函数雅可比矩阵= constvel2djac (~, dt,变长度输入宗量)
%验证状态长4个元素,有两个输入
A1d = [1 dt;0 1];
雅可比矩阵= blkdiag (A1d A1d);
结束
%雅可比矩阵测量功能:
函数雅可比矩阵= cvmeas2DFullStateJac(状态,变长度输入宗量)
%长4个元素验证状态
雅可比矩阵= [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1);
结束
如果把上面的代码添加到“SensorFusionUsingSyntheticDataExample之后。m”文件,initSimDemoFilter函数看起来像下面的:
函数过滤器= initSimDemoFilter(检测)
H = [1 0 0 0;0 0 1 0;0 1 0 0;0 0 0 1);
过滤器= trackingEKF (“状态”,H * detection.Measurement…
“StateTransitionFcn”@constvel2d,…
“MeasurementFcn”@cvmeas2DFullState,…
“StateTransitionJacobianFcn”@constvel2djac,…
“MeasurementJacobianFcn”,@cvmeas2DFullStateJac);
结束
