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