MATLAB 中用于购物车摆系统的扩展卡尔曼滤波器

Question

我已经为购物车摆动力学实现了扩展卡尔曼滤波器（EKF）算法。我面临的问题是估计的状态与真实的状态根本不匹配，甚至不接近。我使用 MATLAB 内置函数来实现该算法，但我不知道我错在哪里。我还检查了

(C,A)

对的可观测性，如果输出矩阵

C

为

[1 0 0 0]

，则可观测性矩阵是满秩的。我真的需要帮助。以下是我的代码：

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

我得到的结果如图所示。

Answer 1

我建议从一个更简单的问题开始。非线性函数使一切变得更加复杂。如果你有一个线性系统，你能让它工作吗？如果是，那就太好了，从那里开始吧。如果不是，那么您将面临更容易的调试挑战。

我还建议不要使用Matlab的内置函数并自己实现矩阵表达式。当 Matlab 的函数可能会执行您意想不到的操作时，调试会变得更加困难。