This is easy to do in OpenCV however I would like a native Matlab implementation that is fairly efficient and can be easily changed. The method should be able to take the camera
You can now do that as of release R2013B, using the Computer Vision System Toolbox. There is a GUI app called Camera Calibrator and a function undistortImage.