CKF计算流程梳理

CKF计算流程梳理

#===========3：t-1时刻容积点计算

#分解成AT*A（下三角*上三角）取下三角
S1 = np.linalg.cholesky(Pk)

#计算容积点,repmat()把元素复制m行n列
#=================================t-1时刻的2n个容积点

#============4：传播容积点，把每一个容积点带入非线性运动状态转移方程，计算出状态向量
####################
for i in range(m):
    
    
    #传播容积点
    
####

#============5：一步预测，t时刻状态向量估计
#状态量预测值

#对每一个预测值，减去容积点
#计算出估计的误差协方差矩阵

#============6:t时刻的容积点计算
#Cholesky分解法又称平方根法
#Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解
#得到t时刻的标准差
S2=np.linalg.cholesky(Ppr)

#得到t时刻的容积点，共有2n个
S2_ke = np.matmul(S2,kesai)

#============7：t时刻容积点非线性传播，目标是计算观测向量

#计算量测预测值      量测=观测向量
#计算出t时刻的观测向量的估计值
Ypre = (1/m)*np.sum(yr,axis=1).reshape(2,1)

#============8：卡尔曼滤波相关参数计算
#计算出观测误差协方差
Pyy=(1/m)*(np.matmul(yr,yr.T)-np.matmul(Ypre,Ypre.T))+R
#状态与观测向量交互协方差
Pxy=(1/m)*(np.matmul(Xp,yr.T) - np.matmul(Xpr,Ypre.T))

#卡尔曼增益
K = np.matmul(Pxy,inv(Pyy))   


#===========9：t时刻的状态估计和协方差计算


#状态估计 
#计算出预测结果                     
Xjian_new=Xpr+np.matmul(K,Y_del)  


#估计误差协方差    
#计算t时刻的协方差矩阵                 

Pk = Ppr-K_Pyy_T

#至此，一轮迭代完成

####################