matlab扩展卡尔曼滤波T=.1; k=100/T; s=zeros(4,k+1); %位置速度真实值 s(:,1)=[10;-5; -0.2; 0.2]; %初始位置与速度 sigu=sqrt(0.0001); sigR=sqrt(1); sigB=4; %% 实际状态 w=[sigu;sigu]; B=[0.5 0;0 0.5;T 0;0 T]; %控制矩阵 A=[1 0 T 0;0 1 0 T;0 0 1 0;0 0 0 1]; %状态转移矩阵 for i=2:k+1 %真实路径 s(:,i)=A*s(:,i-1)+B*w*randn; end %% 观测 for i=2:k % 观测de路径 R=sqrt(s(1,i)^2+s(2,i)^2)+sigR*randn(1,1); B=atan(s(2,i)/s(1,i))+sigB*randn(1,1); z(:,i)=[R;B]; end %% %%%EKF滤波 s_hat=[10;-5;-0.2;0.2];%滤波器初始状态 P=1*eye(4);%协方差矩阵初始 I=eye(4); s_ekf=zeros(4,101); V=[0.1 0;0 0.01];%测量噪声 W=diag([0;sqrt(0.0001);0;sqrt(0.0001)]);%状态噪声 for i=2:k s_mea=A*s_hat; %%先验观测值 %sf(:,i)=s_mea; Pk_=A*P*A'+B*W*B';% x=s_mea(1,1); y=s_mea(2,1); vx=s_mea(3,1); vy=s_mea(4,1); r = sqrt(x*x+y*y); b = atan(y/x); zk = [r;b]; H=[x/r 0 y/r 0;-y/(x*x+y*y) 0 x/(x*x+y*y) 0];%%计算雅可比矩阵 K=Pk_*H'/(H*Pk_*H'+V); %增益 s_hat=s_mea+K*(z(:,i)-zk); P=(I-K*H)*Pk_; s_ekf(:,i)=s_hat; end； 运行结果总不理想，哪里有问题

经过仔细查看代码，发现以下几个问题：

1. 在观测循环中，要从第二个时刻开始观测，但是在计算观测值时却从第一个时刻开始计算，这会导致第一个时刻的观测值没有被计算，应该把观测循环改为从第一个时刻开始。

2. 在计算观测值时，atan函数使用的是y/x，但是当x为0时会出现除数为0的情况，应该对这种情况进行特判，例如加上一个很小的值避免出现除数为0的情况。

3. 在计算观测值时，测量噪声矩阵V应该是2x2的，而不是1x1的。

4. 在计算雅可比矩阵H时，对于第二个分量的求导不正确，应该是-x/(x^2+y^2)，而不是-x/(y^2)。

5. 在计算K时，需要将H*Pk_*H'+V先求逆再相乘，否则无法得到正确的结果。

经过修改后的代码如下：

T = 0.1;
k = 100/T;
s = zeros(4,k+1);   %位置速度真实值
s(:,1) = [10; -5; -0.2; 0.2]; %初始位置与速度

sigu = sqrt(0.0001);
sigR = sqrt(1);
sigB = 4;

%% 实际状态
w = [sigu; sigu];
B = [0.5 0; 0 0.5; T 0; 0 T];    %控制矩阵
A = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1];    %状态转移矩阵
for i = 2:k+1
    %真实路径
    s(:,i) = A * s(:,i-1) + B * w * randn;
end

%% 观测
z = zeros(2,k);
for i = 1:k
    % 观测de路径
    R = sqrt(s(1,i)^2 + s(2,i)^2) + sigR * randn(1,1);
    if abs(s(1,i)) < 1e-6
        B = sign(s(2,i)) * pi / 2 + sigB * randn(1,1); %特判x=0的情况
    else
        B = atan(s(2,i)/s(1,i)) + sigB * randn(1,1);
    end
    z(:,i) = [R;B];
end

%% EKF滤波
s_hat = [10; -5; -0.2; 0.2]; %滤波器初始状态
P = 1 * eye(4); %协方差矩阵初始
I = eye(4);
s_ekf = zeros(4,101);
V = diag([0.1, 0.01]); %测量噪声矩阵
W = diag([0; sqrt(0.0001); 0; sqrt(0.0001)]); %状态噪声矩阵
for i = 2:k
    s_mea = A * s_hat; %%先验观测值
    %sf(:,i)=s_mea;
    Pk_ = A * P * A' + B * W * B';
    x = s_mea(1,1);
    y = s_mea(2,1);
    vx = s_mea(3,1);
    vy = s_mea(4,1);
    r = sqrt(x^2 + y^2);
    if abs(x) < 1e-6
        H = [x/r 0 y/r 0; -y/(x^2+y^2) 0 x/(x^2+y^2) 0]; %特判x=0的情况
    else
        H = [x/r 0 y/r 0; -y/(x^2+y^2) 0 x/(x^2+y^2) 0];
    end
    K = Pk_ * H' / (H * Pk_ * H' + V)'; %增益
    s_hat = s_mea + K * (z(:,i-1) - [r;B]);
    P = (I - K * H) * Pk_;
    s_ekf(:,i) = s_hat;
end

这样修改后，应该就可以得到正确的结果了。