<jupyter_start><jupyter_text>EKF slam and driving through polesThis jupyter notebook will use EKF slam to steer an imaginary rover through some poles. Again leaning heavily on the libraries from the PythonRobotics with some augmentations.<jupyter_code>from SLAM import ekfSlam
from utils import rover
from Control import poseControl
import numpy as np
import matplotlib.pyplot as plt<jupyter_output><empty_output><jupyter_text>The idea will be to identify a "lineup" pose that we should drive to in order to prepare for the rover to drive through the gates, then drive through the gates. Then stop.<jupyter_code>def targetPose(pole1,pole2):
    mid = ((pole1[1]+pole2[1])/2.0,(pole1[1]+pole2[1])/2.0)
    dy = pole2[1]-pole1[1]
    dx = pole2[0]-pole1[0]
    theta = np.arctan2(dx,dy)-np.pi/2
    print(theta)
    return rover(1*np.cos(theta)+mid[0],1*np.sin(theta)+mid[1],theta + np.pi)
    
pole = np.array([[10.0, 10.0],
                        [11.0, 11.0]])
# State Vector [x y yaw v]'
xEst = np.zeros((ekfSlam.STATE_SIZE, 1))
xTrue = np.zeros((ekfSlam.STATE_SIZE, 1))
PEst = np.eye(ekfSlam.STATE_SIZE)

xDR = np.zeros((ekfSlam.STATE_SIZE, 1))  # Dead reckoning

# history
hxEst = xEst
hxTrue = xTrue
hxDR = xTrue

## Setting up simulation.
T = 0.0
rov = rover(0,0,0) # start it at the origin. 
v = 10
yawrate = 10
while(v>0.01 or yawrate>0.01):
    T += ekfSlam.DT
    poseGoal = targetPose(pole[0],pole[1])
    v,yawrate = poseControl.move_to_pose_step(rov,poseGoal)

    u = np.array([[v, yawrate]]).T
    xTrue, z, xDR, ud = ekfSlam.observation(xTrue, xDR, u, pole)

    xEst, PEst = ekfSlam.ekf_slam(xEst, PEst, ud, z)

    x_state = xEst[0:ekfSlam.STATE_SIZE]
    
    rov.x = xEst[0][0]
    rov.y = xEst[1][0]
    rov.theta = xEst[2][0]
    
    # store data history
    hxEst = np.hstack((hxEst, x_state))
    hxDR = np.hstack((hxDR, xDR))
    hxTrue = np.hstack((hxTrue, xTrue))
for i in range(ekfSlam.calc_n_LM(xEst)):
    plt.plot(xEst[ekfSlam.STATE_SIZE + i * 2],
            xEst[ekfSlam.STATE_SIZE + i * 2 + 1], "xg")

plt.plot(hxTrue[0, :],
        hxTrue[1, :], "-b")
plt.plot(hxDR[0, :],
        hxDR[1, :], "-k")
plt.plot(hxEst[0, :],
        hxEst[1, :], "-r")
plt.axis("equal")
plt.grid(True)
plt.pause(0.001)
plt.show()<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.15252747252747254, 'fraction_numerical': 0.035164835164835165, 'mean_word_length': 3.616632860040568, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '2', 'dir': 'jupyter-structured-clean-dedup', 'id': '178033', 'license': 'MIT', 'n_tokens_mistral': 1009, 'n_tokens_neox': 954, 'n_words': 214, 'path': 'nova_rover_demos/.ipynb_checkpoints/EKF Slam steering through poles-checkpoint.ipynb', 'repo_name': 'Liam-Stow/novaRoverDemos', 'size': '2275'}