You can try to cancel out the filter. This can remove lag, but also increases high frequency noise. After doing this, you can try to control the robot based on the new estimate of heading. To do this, you must experiment to work out the low pass filter parameters. For example, in discrete time, you might find:
$$\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$$
where $\hat\theta(t)$ is the estimated heading (compass output) at time $t$, $\theta$ is the actual heading (ground truth) at time $t$.
You can find the parameters $a_i$ by doing an experiment where you measure the ground truth using some other external means. Given $n+k+1$ samples, you have this equation:
$$\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$$
And you can solve by finding:
$$\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$$ where $M^+$ is the pseudo-inverse matrix of $M$. There is no definitive way to work out $k$, so you will probably just guess. For bonus points, this assumes that the noise is white and independent, but you can whiten it first to remove bias, and therefore improve your estimate of the parameters.
You can convert this to a transfer function (also known as Z-transform in the discrete time domain):
$$\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$$
To cancel this out, we can take the inverse (where $\bar\theta(t)$ is our new estimate of heading):
$$\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$$
Converting back to the time domain:
$$a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$$
$$\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$$
we can then use $\bar\theta$ to control the robot.
This will be very noisy, so you might want to still put $\bar\theta$ through a low-pass filter before use (although perhaps one with less lag).
The above solution is still not the best way. The noisy estimate may not be very useful. If we put this into a state space equation, we can design a Kalman filter, and a full-state feedback controller using LQR (linear quadratic regulator). The combination of a Kalman filter and LQR controller is also known as an LQG controller (linear quadratic gaussian), and use loop-transfer recovery to get a good controller.
To do this, come up with the (discrete-time) state-space equations:
$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$, $\vec{y}(t)=C\vec{x}(t)$
or:
$$\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]=
\left[\matrix{
A_1&A_2&\cdots&0&0&0\\
1&0&\cdots&0&0&0\\
0&1&\cdots&0&0&0\\
\vdots&\vdots&&\vdots&\vdots&\vdots\\
0&0&\cdots&1&0&0\\
0&0&\cdots&0&1&0
}\right]
\vec{x}(t-1)
+
\left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$$
$$\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$$
where $\vec{u}(t-1)$ represents the power in the motors to turn the robot, and $A_0$, $A_1$, $B_0$, $B_1$ is how much it affects the heading based on position and speed (you can choose non-zero values for the other elements of the $B$ matrix, and first row of the $A$ matrix too).
Then, you can build your observer (Kalman Filter), by choosing noise estimates $Q_o$ and $R_o$ for the process noise and measurement noise. The Kalman Filter can then find the optimal estimate of heading, given those assumptions about the noise. After choosing the noise estimates, the implementation just depends on implementing code for the Kalman Filter (equations can be found on Wikipedia, so I won't go over it here).
After that, you can design an LQR controller, this time, choosing $Q_c$ and $R_c$ representing the weighting given to regulating the heading, and trying to limit the use of the actuators. In this case, you might choose $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ and $R_c = \left[1\right]$. This is done because LQR finds the optimal controller to minimise a cost function: $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$
Then, you just put it through the discrete time algebraic Riccati equation:
$$P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$$
and solve for a positive definite matrix $P$.
Thus, your control law can be given by:
$$\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$$
where $K = (R + B^T P B)^{-1}(B^T P A)$
Finally, just doing this won't work very well, and is likely to be unstable because of the noise. Indeed, that means option 1 probably won't work unless you first put $\bar\theta$ through a low-pass filter (albeit not necessarily with such a long effective lag time). This is because while LQR is guaranteed stable, as soon as you use a Kalman filter, the guarantee is lost.
To fix this, we use the Loop Transfer Recovery technique, where you adjust the Kalman filter, and instead choose a new $Q_o = Q_0 + q^2BVB^T$, where $Q_0$ is your original $Q$ matrix, tuned so that the Kalman filter is optimal. $V$ is any positive definite symmetric matrix, which you can just choose as the identity matrix ($V=I$). Then, just choose a scalar $q$. The resulting controller should become (more) stable as $q \rightarrow \infty$, although the $Q_o$ matrix becomes de-tuned, which means it becomes less optimal.
Therefore, you just increase $q$ until it is stable. Another way you can try to make it stable, is to increase $R_c$ (or decrease $Q_c$) to make the LQR controller slower.
