Done that 5 years ago (Score 1)

I've developed a quit similar algorithm while I was studying, 5 years ago. We used it here for Indoor navigation purposes (thus GPS won't work anyway) and we've been able to get accuracy down to 2-3 meters (using some kalman filtering and an environment model). Sadly I cannot provide an link to this works because they are all property of the company I work for (Fraunhofer IIS). :(