def send_filt_and_ref(self, data):
# publish the estimated pose:
qest = PlanarSystemConfig()
qest.header.stamp = data.header.stamp
qest.header.frame_id = data.header.frame_id
qest.xm = self.Xest2[0]
qest.ym = self.Xest2[1]
qest.xr = self.Xest2[2]
qest.r = self.Xest2[3]
self.filt_pub.publish(qest)
# publish estimated state
xest = PlanarSystemState()
xest.header.stamp = data.header.stamp
xest.header.frame_id = data.header.frame_id
xest.xm = self.Xest2[0]
xest.ym = self.Xest2[1]
xest.xr = self.Xest2[2]
xest.r = self.Xest2[3]
xest.pxm = self.Xest2[4]
xest.pym = self.Xest2[5]
xest.vxr = self.Xest2[6]
xest.vr = self.Xest2[7]
self.filt_state_pub.publish(xest)
# publish the reference pose:
qest = PlanarSystemConfig()
qest.header.stamp = data.header.stamp
qest.header.frame_id = data.header.frame_id
qest.xm = self.Qref[self.k][0]
qest.ym = self.Qref[self.k][1]
qest.xr = self.Qref[self.k][2]
qest.r = self.Qref[self.k][3]
self.ref_pub.publish(qest)
return