Дойдет до реализации, обращайтесь, поделюсь алгоритмами схожими с вашей задачей. Единственное чего я пока не видел - это алгоритма автонастройки ПИДа реализованного на CS3. Может просто плохо искал.