Extended kalman filter in MATLAB for a cart pendulum system

Question

I have implemented the Extended Kalman Filter (EKF) algorithm for a cart pendulum dynamics. The problem I'm facing is that the estimated states do not match the real states at all, not even close. I'm using the MATLAB built-in functions to implement the algorithm but I do not know where I'm wrong. I have also checked the observability of the pair (C,A) and the observability matrix is of full rank if the output matrix C be[1 0 0 0]. I'm really in need of help. The following is my code:

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')

And the results I'm getting are shown in the figures.

Answer 1

I would suggest starting with a more simple problem. Having the nonlinear functions makes everything more complicated. Can you get it to work if you have a linear system? If yes, great, build up from there. If no, then you have an easier debugging challenge.

I would also suggest not using Matlab's built-in functions and implementing the matrix expressions yourself. It's way harder to debug when Matlab's functions might do things that you don't expect.