Exact Successor State Distribution for a Pendulum

35 Views Asked by At

I want to solve the following problem. Suppose we have a simple pendulum, which follows the differential equation \begin{equation} \dot{x} = f(x) = [x_2, -\sin(x_1)]^T, \text{with } x=[x_1, x_2]^T. \end{equation} Given an inital state the successor state is calculated via eulers method \begin{equation} x_{k+1} = x_k + \Delta t f(x_k), \text{where we use } \Delta t = 1 \text{ for simplicity}. \end{equation} Given $x_k \sim \mathcal{N}(m, S)$, what is the exact distribution of the successor state?

My solution looks like this and is divided into 2 steps.

Step 1) Transform $x \to \dot{x}$ and get $p(\dot{x})$

Step 2) Assume independence and use the convolution rule based on $p(x_k)$ and $p(\dot{x}_k)$


Step 1) We need to solve \begin{equation} p(\dot{x}) = p(x = f^{-1}(\dot{x})) \bigg| \frac{d f^{-1}(\dot{x})}{d \dot{x}} \bigg|. \end{equation} The inverse function gives \begin{equation} f^{-1}(\dot{x}; k, i) = \begin{cases} \begin{bmatrix} \pi + \arcsin \bigl(\dot{x}_2 \bigr) + 2 \pi k & \dot{x}_1 \end{bmatrix}^T, \\ \begin{bmatrix} 2 \pi k - \arcsin \bigl( \dot{x}_2 \bigr) & \dot{x}_1 \end{bmatrix}^T, \end{cases} k \in \mathbb{Z}, -1 \leq \dot{x}_2 \leq 1, \end{equation} and the absolute determinante of the jacobian is \begin{equation} \bigg| \frac{d f^{-1}(\dot{x})}{d \dot{x}} \bigg| = \frac{1}{\sqrt{ |\dot{x}_2^2 - 1| } }, \quad \dot{x}_2 \neq 1. \end{equation} In total we have \begin{equation} p(\dot{x}) = \sum_{i=1}^{2} \sum_{k=-\infty}^{\infty} \frac{p(f^{-1}(\dot{x}); k, i)}{ \sqrt{ |\dot{x}_2^2 - 1| } }. \end{equation}

Here's my matlab code

% - cleanup
clear;
close all;
clc;

% - symbolic variables
syms x1 x2 xp1 xp2 real
x = [x1; x2];
xp = [xp1; xp2];
sol = solve(xp == [x2; -sin(x1)], x, 'ReturnConditions', true);
x1 = sol.x1
x2 = sol.x2
sol.conditions
absdetjac = abs(det(jacobian([x1(1); x2(1)], xp)))

% - grid
grid0.n = 500;
grid0.x1lim = [-10, 10];
grid0.x2lim = [-10, 10];
grid0.x1 = linspace(grid0.x1lim(1), grid0.x1lim(2), grid0.n);
grid0.x2 = linspace(grid0.x2lim(1), grid0.x2lim(2), grid0.n);
grid0.A = (grid0.x1(2)-grid0.x1(1))*(grid0.x2(2)-grid0.x2(1));
[grid0.X1, grid0.X2] = meshgrid(grid0.x1, grid0.x2);
grid0.Xv = [grid0.X1(:), grid0.X2(:)]';

% - initial state distribution
mx = [pi, 0]';
Sx = [2, 0; 0, 0.5];
Lx = chol(Sx)';
grid0.pX = reshape(mvnpdf(grid0.Xv', mx', Sx)', grid0.n, grid0.n);

% - state derivative distribution
fcn = @(x) [x(2, :); -sin(x(1, :))];

invfcn = @(xp, k, i) (i==1)*[pi + asin(xp(2, :)) + 2*pi*k; xp(1, :)] + ...
                     (i==2)*[2*pi*k - asin(xp(2, :)); xp(1, :)];

absdetjacfcn = @(xp) 1./abs(xp(2, :).^2 - 1).^(1/2);

grid0.iok = abs(grid0.Xv(2, :)) <= 1;       % error maybe here?
grid0.k = -3 : 3;
grid0.i = 1 : 2;
grid0.pXpki = zeros(grid0.n, grid0.n, length(grid0.k), length(grid0.i));
for i = 1 : length(grid0.i)
    for j = 1 : length(grid0.k)
        tmp = mvnpdf(...
            invfcn(grid0.Xv, grid0.k(j), grid0.i(i))', mx', Sx) .* ...
            absdetjacfcn(grid0.Xv)';
        tmp(~grid0.iok) = 0;
        grid0.pXpki(:, :, j, i) = reshape(tmp, grid0.n, grid0.n);
    end
end
grid0.pXp = sum(grid0.pXpki, [3, 4]);
grid0.pXX = conv2(grid0.pX, grid0.pXp, 'same');

% - monte carlo approach for reference
seed = 510;
rng(seed);

ns = 150;
nx = 2;
Xs = mx + Lx*randn(nx, ns);
Xps = fcn(Xs);
XXs = Xs + Xps;

% - visualization
figure(1);
subplot(1, 3, 1);
grid on;
hold on;
plot(Xs(1, :), Xs(2, :), 'r.');
contour(grid0.X1, grid0.X2, grid0.pX);

subplot(1, 3, 2);
grid on;
hold on;
plot(Xps(1, :), Xps(2, :), 'r.');
contour(grid0.X1, grid0.X2, grid0.pXp);

subplot(1, 3, 3);
grid on;
hold on;
plot(XXs(1, :), XXs(2, :), 'r.');
contour(grid0.X1, grid0.X2, grid0.pXX);

The resulting plot seems wrong. Where is the problem/mistake?