You are very close to the first half of your goal.
Your line of code
should, I believe, reflect what you are calling Xg in your original problem. You might want to rename it to help you remember the connection between the code and the original equation. Also, you need to look at what the multipliers/coefficients are in your original Xg definition. u(t-1), which you are calling unitstep2 in your code, is multiplied by -2 when assembling the total Xg. This is not reflected in your code.
I think you can figure it out from here.