假設離散時間狀態空間模型為：
普通的擴充套件卡爾曼濾波公式如下：
具體的程式碼可參考：http://blog.csdn.net/u010703122/article/details/49074297
注：
1、先驗估計狀態方程和其雅克比矩陣中的輸入狀態應該是上一時刻的後驗估計狀態；
2、觀測方程中的狀態輸入和其雅克比矩陣的輸入狀態應該是當前時刻的先驗估計狀態；
3、求增益Kk時，注意矩陣逆是否可得