卡尔曼滤波如何调参数使得结果误差最小

你好，同学你对卡尔曼滤波了解的多吗，我这边目前也在学习这方面，遇到了一些问题，不知道方不方便探讨一下

function X= EKF(u1,u2,u3,u4,u5,u6,u7,u8,u9,u10,u11,u12)
a = 1.256;
b = 1.368;
Iz = 2549;
m = 2100;
t = 0.002;
d=1.4;
N=4001;
vx0=80/3.6;vy0=0;wr0=0;
delta=u1;Fx_fl=u2;Fx_fr=u3;Fx_rl=u4;Fx_rr=u5;Fy_fl=u6;Fy_fr=u7;Fy_rl=u8;Fy_rr=u9;ax=u10;ay=u11;r=u12;
persistent P Q R P1 K xxhat xhat
if isempty(P)
P1=zeros(3,3);
K=zeros(3,3);
xxhat=zeros(3,1);
xhat = [vx0,vy0,wr0]';
P =diag([1,1,1]); %等价于 eye(3)
Q =diag([1,1,1]); %等价于 eye(3)
R =diag([1,1,1]); %等价于 eye(3)
end
xhat1=zeros(3,1);
for i=1:N
vx_ekf=xhat(1,i);
vy_ekf=xhat(2,i);
wr_ekf=xhat(3,i);
F_ekf=[0 wr_ekf vy_ekf;
-wr_ekf 0 -vx_ekf;
0 0 0];
A_ekf=[0 wr_ekf/2 vy_ekf/2;
-wr_ekf/2 0 -vx_ekf/2;
0 0 0];
C_ekf=[((Fx_fl+Fx_fr)*cos(delta)+Fx_rl+Fx_rr-(Fy_fl+Fy_fr)*sin(delta))/m;%纵向平衡方程得到纵向加速度
((Fx_fl+Fx_fr)*sin(delta)+Fy_rl+Fy_rr+(Fy_fl+Fy_fr)*cos(delta))/m;%横向平衡方程得到侧向加速度
(a*((Fx_fl+Fx_fr)*sin(delta)+(Fy_fl+Fy_fr)*cos(delta))-b*(Fy_rl+Fy_rr)+((Fx_fr-Fx_fl)*cos(delta)+(Fy_fl-Fy_fr)*sin(delta)+(Fx_rr-Fx_rl))*d/2)/Iz];%横摆角速度
D_ekf=[0 0 0
0 0 0
0 0 1];
%系统离散化处理
fai_ekf=eye(3)+A_ekf*t;
CC_ekf=C_ekf*t;
FF_ekf=eye(3)+F_ekf*t;
xxhat(:,i)=fai_ekf*xhat(:,i)+CC_ekf;%状态预测
P1(:,:,i)=FF_ekf*P(:,:,i)*(FF_ekf)'+Q;%协方差预测
K(:,:,i)=P1(:,:,i)*(D_ekf)'*inv(D_ekf*P1(:,:,i)*(D_ekf)'+R);%卡尔曼增益系数
xhat1(:,i)=xxhat(:,i)+K(:,:,i)*([ax;ay;r]-D_ekf*xxhat(:,i));%最终需要的状态结果
P(:,:,i+1)=(eye(3)-K(:,:,i)*D_ekf)*P1(:,:,i);%更新协方差估计
xhat(:,:,i+1)=xhat1(:,i);
X=xhat1(:,i);
end

上面那个是代码，不过跑起来之后报了矩阵维度的错误，错误信息如下：
Attempted to access index 2 of an array with smaller dimension sizes. The valid index range is 1 to 1. This error will stop the simulation.In MATLAB Function 'zhengchemoxing_EKF/MATLAB Function': P(:,:,i+1)=(eye(3)-K(:,:,i)*D_ekf)*P1(:,:,i);%更新协方差估计Component:MATLAB Function | Category:Runtime errorAn error occurred while running the simulation and the simulation was terminatedCaused by:
Simulation stopped because of a runtime error.

有没有对这个比较熟悉点的大佬烦请指点一下了，在此谢过诸位