def __init__(self):
# Open the file descriptors
BUFSIZE = 1024
self.mb100log = open("logs/mb100.log", 'wb', BUFSIZE)
self.mb100rcv = serial.Serial("/dev/mb100rover", 115200, timeout=0.04)
# Static rotation matrix
self.klingen = sio.loadmat('klingenberg.mat')
self.fjorden = sio.loadmat('fjorden.mat')
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'],
self.klingen['rotlat'])
self.Re2n = self.Rn2e.T
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'],
self.klingen['rotlon'])
# Create objects for the kftrack stuff
self.kftrackpath = rospy.Publisher('kftrack', Path, queue_size=3)
self.kftrackmsg = Path()
self.kftrackmsg.header.frame_id = "ned"
# Initialise pose for the graphic path segment for rviz
h = Header()
p = Point(0, 0, 0)
q = Quaternion(0, 0, 0, 1)
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.x_hat = np.zeros(17)
def __init__(self):
self.thrustdiff = 0
self.tau = np.zeros(5) # input vector
self.x = np.zeros(17) # state vector
self.x[0] = -40
self.x[1] = -50
self.x_hat = self.x
self.v = np.array([0.1,0.1,13.5969e-006,0.2,0.2,0.00033,0.00033])#Measurement,noise
self.z = np.zeros(7)
self.P_plus = np.zeros([17,17])
self.R = np.diag(self.v)
self.R_i = np.diag(self.v)
self.old_z = np.ones(7) # Used for calculation of speed over ground and track angle for the GPS
# Construct kalmanfilterfoo, because it contains the aaushipsimmodel function
self.f = kfoo.KF()
self.roll = 0
self.pitch = 0
self.yaw = 0
self.rightthruster = 0
self.leftthruster = 0
rospy.init_node('simulation_node')
self.r = rospy.Rate(20) # Hz
self.sub = rospy.Subscriber('lli_input', LLIinput, self.llicb)
self.pub = rospy.Publisher('kf_states', Float64MultiArray, queue_size=1) # This should eventually be removed when the kf-node is tested against this
self.subahrs = rospy.Subscriber('attitude', Quaternion, self.ahrscb) # Should be removed from here when the kf-node is tested against this
self.pubimu = rospy.Publisher('imu', ADIS16405, queue_size=1, latch=True)
self.trackpath = rospy.Publisher('track', Path, queue_size=3)
self.pubgps1 = rospy.Publisher('gps1', GPS, queue_size=1, latch=False)
# Construct variables for messages
self.imumsg = ADIS16405()
self.pubmsg = Float64MultiArray()
self.trackmsg = Path()
self.trackmsg.header.frame_id = "ned"
self.gpsmsg = GPS()
# Load external staitc map and path data
self.klingen = sio.loadmat('klingenberg.mat')
self.path = sio.loadmat('../../../../../matlab/2mmargintrack.mat')
# Static rotation matrix
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'], self.klingen['rotlat'])
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'], self.klingen['rotlon'])
# Variables for the thrusters
self.leftthruster = 0.0
self.rightthruster = 0.0
h = Header()
q = Quaternion(0,0,0,1)
def __init__(self):
# Load discretised model constants
self.ssmat = sio.loadmat('../../../../../matlab/ssaauship.mat')
self.tau = np.zeros(5) # input vector
self.x = np.zeros(17) # state vector
self.x[0] = 20
self.x_hat = self.x
self.x_hat[0] = -40
self.x_hat[1] = -50
# Measurement noise vector and covarince matrix
self.v = np.array([0.1,0.1,13.5969e-006,0.2,0.2,0.033,0.033])#Measurement,noise
self.P_plus = np.zeros([17,17])
self.R = np.diag(self.v)
self.R_i = np.diag(self.v)
# Initialisation of thruster input variables
self.rightthruster = 0
self.leftthruster = 0
# Construct kalmanfilterfoo, because it contains the aaushipsimmodel function
self.f = kfoo.KF()
# Process noise vector and covariance matrix
self.w = np.array([0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.001, 0.01, 0.01, 0.01, 0.01, 0.01, 0.033, 0.033, 0.033, 0.033, 0.033])
self.Q = np.diag(self.w)
self.no_of_states = 17
# Measurement vector
self.z = np.zeros(7)
self.z[0] = 20
self.kftrackpath = rospy.Publisher('kftrack', Path, queue_size=3)
self.kftrackmsg = Path()
self.kftrackmsg.header.frame_id = "ned"
# Static rotation matrix
self.klingen = sio.loadmat('klingenberg.mat')
self.Rn2e = geo.RNED2ECEF(self.klingen['rotlon'], self.klingen['rotlat'])
self.Re2n = self.Rn2e.T
self.pos_of_ned_in_ecef = geo.wgs842ecef(self.klingen['rotlat'], self.klingen['rotlon'])
self.fjorden = sio.loadmat('fjorden.mat')
# Define the path poses for the map to display in rviz
self.refmsg = Path()
self.refmsg.header.frame_id = "ned"
self.keepoutmsg = Path()
self.keepoutmsg.header.frame_id = "ned"
q = Quaternion(0,0,0,1)
h = Header()
offset = 3
for i in self.klingen['outer']:
p = Point(i[0]-offset,i[1],0)
self.refmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.klingen['inner']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
for i in self.fjorden['all']:
p = Point(i[0]-offset,i[1],0)
self.keepoutmsg.poses.append(PoseStamped(h, Pose(p, q)))
# Topics
self.subgps1 = rospy.Subscriber('gps1', GPS, self.gps1cb)
self.subgps2 = rospy.Subscriber('gps2', GPS, self.gps2cb)
self.subimu = rospy.Subscriber('imu', ADIS16405, self.imucb)
self.subahrs = rospy.Subscriber('attitude', Quaternion, self.ahrscb)
self.sub = rospy.Subscriber('lli_input', LLIinput, self.llicb)
self.pub = rospy.Publisher('kf_statesnew', Float64MultiArray, queue_size=1)
self.refpath = rospy.Publisher('refpath', Path, queue_size=3, latch=True)
self.keepoutpath = rospy.Publisher('keepout', Path, queue_size=3, latch=True)
# Initialise pose for the graphic path segment for rviz
h = Header()
p = Point(0,0,0)
q = Quaternion(0,0,0,1)
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
self.kftrackmsg.poses.append(PoseStamped(h, Pose(p, q)))
# Initialize common variables
self.roll = 0
self.pitch = 0
self.yaw = 0
self.rightthruster = 0
self.leftthruster = 0
rospy.init_node('klamanfilter_node')