MATLAB小车摆系统EKF状态估计完全失配问题求助

已为小车摆系统实现扩展卡尔曼滤波（EKF）算法，当前遇到估计状态与真实状态完全不匹配的问题。使用MATLAB内置函数实现，但无法定位问题根源。已验证当输出矩阵C为[1 0 0 0]时，(C,A)对的可观性矩阵满秩。附上实现代码及结果图，寻求技术帮助：

clc;clear;close all;

m1 = 1;
m2 = 0.5;
L = 1.5;
g = 9.81;

Ts = 0.001;
tMax = 20;
t = 0:Ts:tMax;
N = numel(t);
c = @(x) cos(x);
s= @(x) sin(x);

f = @(x,u) Ts*[x(3)
                    x(4)
                    (m2*g*c(x(2))*s(x(2))+m2*L*x(4)^2*s(x(2)))/(m1+m2*s(x(2))^2)
                    -(g/L)*s(x(2))-(m2*g*s(x(2))*c(x(2))^2)/(L*(m1+m2*s(x(2))^2))-c(x(2))*m2*x(4)^2*s(x(2))/(m1+m2*s(x(2))^2)] + ...
                ...
                Ts*[0
                      0
                     1/(m1+m2*s(x(2))^2)
                    -c(x(2))/(L*(m1+m2*s(x(2))^2))]*u;


ff = @(x) [x(3)
                x(4)
    (m2*g*c(x(2))*s(x(2))+m2*L*x(4)^2*s(x(2)))/(m1+m2*s(x(2))^2)
    -(g/L)*s(x(2))-(m2*g*s(x(2))*c(x(2))^2)/(L*(m1+m2*s(x(2))^2))-c(x(2))*m2*x(4)^2*s(x(2))/(m1+m2*s(x(2))^2)];

GG = @(x)[0
                 0
                1/(m1+m2*s(x(2))^2)
               -c(x(2))/(L*(m1+m2*s(x(2))^2))];

h = @(x,u) x(1)+0*u;

n = 4;      % NUmber of states

x = zeros(n,N);
x(:,1) = [2 0 0 0]';
xHat = x;
xHat(:,1) = [0 1 1 1]';

m = 1;      % Number of Outputs
y = zeros(m,N);
yHat = y; 

rw = 0.1;
Rw = rw*diag([1 1 1 1]);
Rv = 0.1;
% rng default
OutputNoiseVector = Rv*ones(m,1);
Rv = diag(OutputNoiseVector);
w = sqrt(Rw)*randn(n,N);
v = sqrt(Rv)*randn(m,N);

Domain = 2;
Omega = pi/3;
u = Domain*sin(Omega*t);

I = eye(n);
P0 = 1e5*I;
xReal = x;

obj = extendedKalmanFilter(f,h,xHat(:,1));
obj.ProcessNoise = Rw;
obj.MeasurementNoise = Rv;

for i=2:N

    x(:,i) = x(:,i-1) + Ts*(ff(x(:,i-1))+GG(x(:,i-1))*u(:,i-1))+w(:,i-1);
    xReal(:,i) = xReal(:,i-1) + Ts*(ff(xReal(:,i-1))+GG(xReal(:,i-1))*u(:,i-1));

    %% Prediction

    predict(obj,u(i-1));

    %% Correction

    y = h(x(:,i),u(:,i-1));
    correct(obj,y,u(i-1));

    xHat(:,i) = obj.State;

end

%% Plot

Namex = {'x','$\theta$','$\dot{x}$','$\dot{\theta}$'};
namexHat = {'$\hat{x}$','$\hat{\theta}$','$\hat{x}dot$','$\hat{\theta}Dot$'};
Real_x_Name = {'$x_{Real}$','$\theta_{Real}$','$\dot{x_{Real}}$','$\dot{\theta_{Real}}$'};

f1 = figure(1);

for i=1:n

    subplot(2,2,i)
    plot(t,x(i,:),'b','LineWidth',2)
    hold on
    grid on
    xlabel('Time (s)','Interpreter','Latex')
    plot(t,xHat(i,:),'r','LineWidth',1.75)
    hold on
    plot(t,xReal(i,:),'--','LineWidth',2)
    legend(Namex{i},namexHat{i},Real_x_Name{i},'InterPreter','Latex')

end

f2 = figure(2);

for i=1:n

    subplot(2,2,i)
    plot(t,xHat(i,:),'--','LineWidth',1.75)
    hold on
    plot(t,xReal(i,:),'LineWidth',2)
    xlabel('Time (s)','Interpreter','Latex')
    legend(namexHat{i},Real_x_Name{i},'InterPreter','Latex')

end

movegui(f1,'east')
movegui(f2,'west')

结果图：

内容的提问来源于stack exchange，提问作者MMd.NrC