# Compute a K-step GM-estimate of regression, 
# using Huber's psi function or the bisquare,
# and Newton-Raphson.
# Load the robustbase package first: 
library(robustbase)

myGM = function(x, y, type="Huber", c=1.5, intercept, wts = "w1", K=3) {
x = as.matrix(x)
n = length(y)
p = ncol(x) + (intercept==T)
if(intercept==T) X = cbind(1,x) else X=x

if(type=="Huber") {
# Define Huber's psi function, with tuning constant 'c':
psi = function(r) pmax(-c, pmin(r,c))
psiprime = function(r) (abs(r)<=c)
}

if(type=="bisquare") {
# Define the bisquare psi function, with tuning constant 'c':
psi = function(r) (abs(r)<=c)*r*(1-(r/c)^2)^2
psiprime = function(r) (abs(r)<=c)*((1-(r/c)^2))*((1-5*(r/c)^2))
}

# Define the robust weights
w = function(x) {
qwe = covMcd(x)
if(ncol(x)>1) mah = qwe$mah else 
mah = (x-rep(qwe$center,length(x)))^2/as.numeric(qwe$cov)
if(wts=="w1") weights = sqrt(pmin(1, qchisq(.95, ncol(x))/mah)) else
if(wts=="w2") weights = pmax(0, (1-(mah/qchisq(.95, ncol(x)))^3)^3)
 #Alternate weights, cutting influence of outlying xs to zero
weights
	}

# eta and etaprime
eta = function(wx,r) wx*psi(r)
etaprime = function(wx,r) wx*psiprime(r)

# Arrange to store the output:
out = matrix(ncol = p+1)
if(intercept==T) dimnames(out) = list(NULL, c(paste("theta", 0:(p-1), sep=""), "sigma")) else
dimnames(out) = list(NULL, c(paste("theta", 1:p, sep=""), "sigma"))

# Start with the LTS estimate
init.fit = ltsReg(x,y, intercept = intercept)
theta = init.fit$coef
r = init.fit$resid
sigma = init.fit$scale #Initial scale estimate
std.res = r/sigma
wx = w(x)
out[1,] = round(c(theta, sigma),5)

for  (k in 1:K) { #Solve G(theta)=0 by Newton-Raphson
G = t(X)%*%eta(wx,std.res)/n
Gdot = (-1/(n*sigma))*t(X)%*%(as.vector(etaprime(wx,std.res))*X)
Gdot.qr = qr(Gdot)
theta = theta-qr.solve(Gdot.qr,G)
r = y-X%*%theta
sigma = mad(r, center=0)
std.res = r/sigma
out = rbind(out, round(c(theta, sigma),5))
	}
print(wx)
list(out=out, theta=theta, wx=wx, std.res = std.res, coef = theta, sigma = sigma)
}