mirror of
https://github.com/AtsushiSakai/PythonRobotics.git
synced 2026-01-14 00:27:55 -05:00
keep implementing
This commit is contained in:
Atsushi Sakai
@@ -10,69 +10,14 @@ import numpy as np
|
||||
import math
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
# Estimation parameter
|
||||
Q = np.diag([0.5, 0.5])**2
|
||||
R = np.diag([1.0, math.radians(30.0)])**2
|
||||
|
||||
# Covariance Matrix for motion
|
||||
# Q=diag([0.1 0.1 toRadian(1) 0.05]).^2;
|
||||
# Simulation parameter
|
||||
Qsim = np.diag([0.5, 0.5])**2
|
||||
Rsim = np.diag([1.0, math.radians(30.0)])**2
|
||||
|
||||
# % Covariance Matrix for observation
|
||||
# R=diag([1.5 1.5 toRadian(3) 0.05]).^2;
|
||||
|
||||
# % Simulation parameter
|
||||
# global Qsigma
|
||||
# Qsigma=diag([0.1 toRadian(20)]).^2; %[v yawrate]
|
||||
|
||||
# global Rsigma
|
||||
# Rsigma=diag([1.5 1.5 toRadian(3) 0.05]).^2;%[x y z yaw v]
|
||||
|
||||
# PEst = eye(4);
|
||||
|
||||
# u=doControl(time);
|
||||
# % Observation
|
||||
# [z,xTrue,xd,u]=Observation(xTrue, xd, u);
|
||||
|
||||
# % ------ Kalman Filter --------
|
||||
# % Predict
|
||||
# xPred = f(xEst, u);
|
||||
# F=jacobF(xPred, u);
|
||||
# PPred= F*PEst*F' + Q;
|
||||
|
||||
# % Update
|
||||
# H=jacobH(xPred);
|
||||
# y = z - h(xPred);
|
||||
# S = H*PPred*H' + R;
|
||||
# K = PPred*H'*inv(S);
|
||||
# xEst = xPred + K*y;
|
||||
# PEst = (eye(size(xEst,1)) - K*H)*PPred;
|
||||
|
||||
# % Simulation Result
|
||||
# result.time=[result.time; time];
|
||||
# result.xTrue=[result.xTrue; xTrue'];
|
||||
# result.xd=[result.xd; xd'];
|
||||
# result.xEst=[result.xEst;xEst'];
|
||||
# result.z=[result.z; z'];
|
||||
# result.PEst=[result.PEst; diag(PEst)'];
|
||||
# result.u=[result.u; u'];
|
||||
|
||||
# %Animation (remove some flames)
|
||||
# if rem(i,5)==0
|
||||
# %hold off;
|
||||
# plot(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on;
|
||||
# plot(result.z(:,1),result.z(:,2),'.g');hold on;
|
||||
# plot(result.xd(:,1),result.xd(:,2),'.k');hold on;
|
||||
# plot(result.xEst(:,1),result.xEst(:,2),'.r');hold on;
|
||||
# ShowErrorEllipse(xEst,PEst);
|
||||
# axis equal;
|
||||
# grid on;
|
||||
# drawnow;
|
||||
# %movcount=movcount+1;
|
||||
# %mov(movcount) = getframe(gcf);% アニメーションのフレームをゲットする
|
||||
# end
|
||||
# end
|
||||
# toc
|
||||
# %アニメーション保存
|
||||
# %movie2avi(mov,'movie.avi');
|
||||
|
||||
# DrawGraph(result);
|
||||
|
||||
# function ShowErrorEllipse(xEst,PEst)
|
||||
# %誤差分散円を計算し、表示する関数
|
||||
@@ -154,35 +99,6 @@ import matplotlib.pyplot as plt
|
||||
# 0 0 0 1];
|
||||
|
||||
|
||||
# function [z, x, xd, u] = Observation(x, xd, u)
|
||||
# %Calc Observation from noise prameter
|
||||
# global Qsigma;
|
||||
# global Rsigma;
|
||||
|
||||
# x=f(x, u);% Ground Truth
|
||||
# u=u+Qsigma*randn(2,1);%add Process Noise
|
||||
# xd=f(xd, u);% Dead Reckoning
|
||||
# z=h(x+Rsigma*randn(4,1));%Simulate Observation
|
||||
|
||||
|
||||
# function []=DrawGraph(result)
|
||||
# %Plot Result
|
||||
|
||||
# figure(1);
|
||||
# x=[ result.xTrue(:,1:2) result.xEst(:,1:2) result.z(:,1:2)];
|
||||
# set(gca, 'fontsize', 16, 'fontname', 'times');
|
||||
# plot(x(:,5), x(:,6),'.g','linewidth', 4); hold on;
|
||||
# plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;
|
||||
# plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;
|
||||
# plot(result.xd(:,1), result.xd(:,2),'--k','linewidth', 4); hold on;
|
||||
|
||||
# title('EKF Localization Result', 'fontsize', 16, 'fontname', 'times');
|
||||
# xlabel('X (m)', 'fontsize', 16, 'fontname', 'times');
|
||||
# ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times');
|
||||
# legend('Ground Truth','GPS','Dead Reckoning','EKF','Error Ellipse');
|
||||
# grid on;
|
||||
# axis equal;
|
||||
|
||||
# function angle=Pi2Pi(angle)
|
||||
# %ロボットの角度を-pi~piの範囲に補正する関数
|
||||
# angle = mod(angle, 2*pi);
|
||||
@@ -194,14 +110,6 @@ import matplotlib.pyplot as plt
|
||||
# angle(i) = angle(i) + 2*pi;
|
||||
|
||||
|
||||
# function radian = toRadian(degree)
|
||||
# % degree to radian
|
||||
# radian = degree/180*pi;
|
||||
|
||||
# function degree = toDegree(radian)
|
||||
# % radian to degree
|
||||
# degree = radian/pi*180;
|
||||
|
||||
DT = 0.1 # time tick [s]
|
||||
SIM_TIME = 60.0 # simulation time [s]
|
||||
|
||||
@@ -215,12 +123,21 @@ def do_control():
|
||||
return u
|
||||
|
||||
|
||||
# z, xTrue, xd, u = Observation(xTrue, xd, u)
|
||||
def observation(xTrue, xd, u):
|
||||
|
||||
xTrue = motion_model(xTrue, u)
|
||||
|
||||
return xTrue
|
||||
zx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]
|
||||
zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]
|
||||
z = np.matrix([zx, zy])
|
||||
|
||||
ud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]
|
||||
ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]
|
||||
ud = np.matrix([ud1, ud2]).T
|
||||
|
||||
xd = motion_model(xd, ud)
|
||||
|
||||
return xTrue, z, xd, ud
|
||||
|
||||
|
||||
def motion_model(x, u):
|
||||
@@ -240,29 +157,49 @@ def motion_model(x, u):
|
||||
return x
|
||||
|
||||
|
||||
def ekf_estimation(xEst, PEst, u):
|
||||
|
||||
# Predict
|
||||
xPred = motion_model(xEst, u)
|
||||
# F=jacobF(xPred, u);
|
||||
# PPred= F*PEst*F' + Q;
|
||||
|
||||
# Update
|
||||
# H=jacobH(xPred);
|
||||
# y = z - h(xPred);
|
||||
# S = H*PPred*H' + R;
|
||||
# K = PPred*H'*inv(S);
|
||||
# xEst = xPred + K*y;
|
||||
xEst = xPred
|
||||
# PEst = (eye(size(xEst,1)) - K*H)*PPred;
|
||||
|
||||
return xEst
|
||||
|
||||
|
||||
def main():
|
||||
print(__file__ + " start!!")
|
||||
|
||||
time = 0.0
|
||||
# State Vector [x y yaw v]'
|
||||
# xEst = np.matrix(np.zeros((3, 1)))
|
||||
xEst = np.matrix(np.zeros((4, 1)))
|
||||
xTrue = np.matrix(np.zeros((4, 1)))
|
||||
PEst = np.eye(4)
|
||||
|
||||
# Dead Reckoning
|
||||
xDR = np.matrix(np.zeros((4, 1)))
|
||||
|
||||
# Observation vector
|
||||
# z = np.matrix(np.zeros((2, 1)))
|
||||
|
||||
while SIM_TIME >= time:
|
||||
# print(time)
|
||||
time += DT
|
||||
|
||||
u = do_control()
|
||||
|
||||
xTrue = observation(xTrue, xDR, u)
|
||||
xTrue, z, xDR, ud = observation(xTrue, xDR, u)
|
||||
|
||||
plt.plot(xTrue[0, 0], xTrue[1, 0], ".r")
|
||||
xEst = ekf_estimation(xEst, PEst, ud)
|
||||
|
||||
plt.plot(xTrue[0, 0], xTrue[1, 0], ".b")
|
||||
plt.plot(xDR[0, 0], xDR[1, 0], ".k")
|
||||
plt.plot(z[0, 0], z[0, 1], ".g")
|
||||
plt.plot(xEst[0, 0], xEst[1, 0], ".r")
|
||||
plt.axis("equal")
|
||||
plt.grid(True)
|
||||
plt.pause(0.001)
|
||||
|
||||
Reference in New Issue
Block a user