Home>

I am learning programming of self-position estimation of an autonomous robot using Kalman filter using MATLAB.
As a feature of my code,

-The input to the robot is not the speed and angle (v, θ), but the attraction (velocity vector) V = (Vx, Vy) from the current position to the target position. The state transition formula is X (t + 1) = F * X (t) + a * V. (A is the descent coefficient)
・ Observation data is set so that one point is set as a landmark and the distance and angle from the current position to the landmark can be obtained as data.

The problem is that the position that can be estimated correctly halfway goes out of control after the step that minimizes the distance from the landmark.
(See figure) Blue point: actual robot position, red point: Kalman filter estimated position, green point: target position of the robot, black point: landmark blue ellipse: estimation error ellipse.

Probably, the angle processing (atan2) in the observation data seems to be incomplete, but it cannot be solved easily.

If i am familiar with Kalman filter, I would like advice.
I'll put the source code too, but I don't think it's very clean code ...
I refer to this page well. ↓
https://myenigma.hatenablog.com/entry/20130413/1365826157

function [] = VectorFieldKalmanFilterLocalization ()
close all;
clear all;
disp ('Vector Field Kalman Filter sample program start !!')
global xg
xg = [20;20];
global cg
cg = 300;
global lg
lg = 100;
global a
a = 0.1;
x0 = [0;0];
global LM
LM = [15;10];

% plot (x0 (1), x0 (2), 'o', 'MarkerSize', 10, 'MarkerFaceColor', 'y');hold on;
plot (xg (1), xg (2), 'o', 'MarkerSize', 10, 'MarkerFaceColor', 'g');hold on;
plot (LM (1), LM (2), 'o', 'MarkerSize', 15, 'MarkerFaceColor', 'k');hold on;

result.xTrue = [];
result.xd = [];
result.xEst = [];
result.z = [];
result.PEst = [];
result.u = [];
Xact = [0;5];
Xdes = [0;0];
Xest = [0;5];
global z
z = [0;0];
global F
F = eye (2);
H = eye (2);
Pest = eye (2);
w = [0.01;0.02];
v = [0.03;toRadian (1)];
Q = diag (w). ^ 2;
R = diag (v). ^ 2;
% Main loop
for t = 1: 300

    % Target trajectory vector
    % Vdes = VerocityVector (Xdes);
    % Target trajectory
    % Xdes = f (Xdes, Vdes);
    % Real trajectory vector
    Vact = VerocityVectorIncludeNoise (Xact, w);
    % Real trajectoryXact = f (Xact, Vact);
    % Predicted trajectory vector
    Vest = VerocityVector (Xest);
    % Predicted orbit and covariance
    Xpred = f (Xest, Vest);
    [vxdx vxdy vydx vydy] = bibun (Vest, Xest, xg);
    Ft = jacobF (vxdx, vxdy, vydx, vydy);
    Ppred = Ft * Pest * Ft '+ Q;
    % Observed value
    z = Observation (Xact, LM, v);
    zpred = Observation (Xpred, LM, v * 0);
   % Kalman gain calculation
    y = z-zpred;
    Ht = JacobianH (Xpred, LM);
    S = R + Ht * Ppred * Ht ';
    K = Ppred * Ht '* inv (S);
    % Updated value
    Xest = Xpred + K * y ';
    Pest = (eye (2)-K * Ht) * Ppred;
    figure (1);
    set (gca, 'fontsize', 16, 'fontname', 'times');
    % plot (Xdes (1), Xdes (2), '. k', 'linewidth', 7);hold on;
    plot (Xact (1), Xact (2), '. b', 'linewidth', 7);hold on;
    plot (Xest (1), Xest (2), '. r', 'linewidth', 7);hold on;
    title ('EKF Localization Result', 'fontsize', 16, 'fontname', 'times');
    xlabel ('X (m)', 'fontsize', 16, 'fontname', 'times');
    ylabel ('Y (m)', 'fontsize', 16, 'fontname', 'times');
    grid on;
    axis equal;

    if rem (t, 5) == 0
     ShowErrorEllipse (Xest, Pest);
    end
    drawnow;
end


function ShowErrorEllipse (Xest, Pest)
Function to calculate and display% error variance circle
Pxy = Pest (1: 2,1: 2);get covariance of% x, y
[eigvec, eigval] = eig (Pxy);Calculate% eigenvalues ​​and eigenvectors
Find the index with the higher eigenvalue
if eigval (1,1)>= eigval (2,2)
    bigind = 1;
    smallind = 2;
else
    bigind = 2;
    smallind = 1;
end
chi = 9.21;% square distribution of error ellipse chi 99%
% Ellipse depiction
t = 0: 10: 360;
a = sqrt (eigval (bigind, bigind) * chi);
b = sqrt (eigval (smallind, smallind) * chi);
x = [a * cosd (t);
   b * sind (t)];
Calculate the angle of the% error ellipse
angle = atan2 (eigvec (bigind, 2), eigvec (bigind, 1));
if (angle<0)
    angle = angle + 2 * pi;end
Rotate% error ellipse
R = [cos (angle) sin (angle);
   -sin (angle) cos (angle)];
x = R * x;
plot (x (1,:) + Xest (1), x (2,:) + Xest (2))
function V = VerocityVector (x)
Calculate velocity vector at% position x
global xg;
global cg;
global lg;
Vx =-(2 * cg * (x (1) -xg (1))/lg ^ 2) * exp (-(((x (1) -xg (1)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2);
Vy =-(2 * cg * (x (2) -xg (2))/lg ^ 2) * exp (-(((x (2) -xg (2)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2);
V = [Vx;Vy];
function V = VerocityVectorIncludeNoise (x, w)
Calculation of velocity vector considering noise at% position x
global xg;
global cg;
global lg;
Vx =-(2 * cg * (x (1) -xg (1))/lg ^ 2) * exp (-(((x (1) -xg (1)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2) + w (1) * randn;
Vy =-(2 * cg * (x (2) -xg (2))/lg ^ 2) * exp (-(((x (2) -xg (2)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2) + w (2) * randn;
V = [Vx;Vy];
function x1 = f (x, v)
% Operation model
global a;
x1 = x + v * a;
function z = Observation (x, xm, v)
% Measurement model
z (1) = ((xm (1) -x (1)) ^ 2+ (xm (2) -x (2)) ^ 2) ^ 0.5 + v (1) * randn;
z (2) = PI2PI (atan2 (xm (2) -x (2), xm (1) -x (1))) + v (2) * randn;
% z (1) = x (1) + v (1) * randn;
% z (2) = x (2) + v (2) * randn;
function angle = PI2PI (angle)
    angle = atan2 (sin (angle), cos (angle));

function Ft = jacobF (vxdx, vxdy, vydx, vydy)
% Jacobian of Motion Model
global F;
global a;
Ft = F + a * [vxdx vxdy;vydx vydy];
function Ht = JacobianH (x, LM)
    Ht = [
        -(LM (1) -x (1))/((LM (1) -x (1)) ^ 2+ (LM (2) -x (2)) ^ 2) ^ 0.5,-(LM (2 ) -x (2))/((LM (1) -x (1)) ^ 2+ (LM (2) -x (2)) ^ 2) ^ 0.5;
        -(LM (2) -x (2))/((LM (2) -x (2)) ^ 2+ (LM (1) -x (1)) ^ 2),-(LM (1)- x (1))/((LM (2) -x (2)) ^ 2+ (LM (1) -x (1)) ^ 2)];

function [vxdx vxdy vydx vydy] = bibun (v, x, xg)
global a;
global cg;
global lg;
vxdx =-((2 * cg * a)/lg ^ 2) * (exp (-(((x (1) -xg (1)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2)) * (1 + ((x (1) -xg (1)) * v (1))/2 * a * cg);
vxdy = -v (1) * (2 * (x (2) -xg (2))/lg ^ 2);
vydx = -v (2) * (2 * (x (1) -xg (1))/lg ^ 2);
vydy =-((2 * cg * a)/lg ^ 2) * (exp (-(((x (1) -xg (1)) ^ 2) + (x (2) -xg (2)) ^ 2)/lg ^ 2)) * (1 + ((x (2) -xg (2)) * v (2))/2 * a * cg);

function radian = toRadian (degree)
% degree to radian
radian = degree/180 * pi;

  • Answer # 1

    The Jacobian matrix of the observation of the question sentence is organized

    dx = LM (1) -x (1)
    dy = LM (2) -x (2)
    r2 = dx ^ 2 + dy ^ 2;
    Ht = [
    -dy/(r2) ^ 0.5, -dx/(r2) ^ 0.5;
    -dx/(r2), -dy/(r2)];

    but
    Correctly

    dx = LM (1) -x (1)
    dy = LM (2) -x (2)
    r2 = dx ^ 2 + dy ^ 2;
    Ht = [
    -dy/(r2) ^ 0.5, dx/(r2) ^ 0.5;
    dx/(r2), dy/(r2)];
    Isn't it

    ?

    Reference

Trends