Your problem is merely a weighted least square optimization. There are many packages available in R on weighted least squared estimate. Nonparametric methods like local polynomial, smoothing spline, etc, have been integrated into R. You may use like the "locpol" package. From your code, I guess that you could be just a beginner. When doing programming, you'd better to reduce the number of loops, otherwise, computation time would be a big trouble when the sample size is very large.