Kalman滤波公式及过程

状态空间模型描述的动态系统

其中k为离散时间，系统在时刻k的状态为， 为对应状态的观测信号，为输入的白噪声， 为观测噪声。为状态转移矩阵，为观测矩阵。

状态一步预测和误差

不考虑噪声的时候

输出信号估计值与实际值有误差

%0A#card=math&code=%7B%7B%5Cbf%7B%5Chat%20x%7D%7Dk%7D%20%3D%20%7B%7B%5Cbf%7BA%7D%7D_k%7D%7B%7B%5Cbf%7B%5Chat%20x%7D%7D%7Bk%20-%201%7D%7D%20%2B%20%7B%7B%5Cbf%7BH%7D%7Dk%7D%5Cleft%28%20%7B%7B%7B%5Cbf%7By%7D%7D_k%7D%20-%20%7B%7B%5Cbf%7BC%7D%7D_k%7D%7B%7B%5Cbf%7BA%7D%7D_k%7D%7B%7B%7B%5Cbf%7B%5Chat%20x%7D%7D%7D%7Bk%20-%201%7D%7D%7D%20%5Cright%29%0A)

滤波增益矩阵

%5E%7B%20-%201%7D%7D%0A#card=math&code=%7B%7B%5Cbf%7BG%7D%7Dk%7D%20%3D%20%7B%7B%5Cbf%7BH%7D%7D_k%7D%20%3D%20%7B%7B%5Cbf%7BP%7D%7D%7Bk%7B%5Crm%7B%7Ck%20-%201%7D%7D%7D%7D%7B%5Cbf%7BC%7D%7Dk%5EH%7B%5Cleft%28%20%7B%7B%7B%5Cbf%7BC%7D%7D_k%7D%7B%7B%5Cbf%7BP%7D%7D%7Bk%7B%5Crm%7B%7Ck%20-%201%7D%7D%7D%7D%7B%5Cbf%7BC%7D%7D_k%5EH%20%2B%20%7B%7B%5Cbf%7BR%7D%7D_k%7D%7D%20%5Cright%29%5E%7B%20-%201%7D%7D%0A)

一步预测协方差阵

协方差阵更新

具体推导过程参考

https://www.yuque.com/g/uestceagles/xb9prl/folder/20733869