文章目录一、卡尔曼滤波的本质与适用场景核心特点适用前提二、卡尔曼滤波的数学模型1、状态方程与观测方程2、卡尔曼滤波两步法五大方程预测(时间更新)更新(测量更新)卡尔曼滤波Kalman Filter是嵌入式开发中极为核心的状态估计算法尤其适用于存在噪声的传感器数据融合和动态系统状态估计。一、卡尔曼滤波的本质与适用场景卡尔曼滤波是一种最优递归数据融合算法在线性高斯假设下通过预测系统模型与更新测量两个步骤不断迭代得到系统状态的最小均方误差估计。核心特点递推计算无需存储全部历史数据适合嵌入式实时系统融合多个噪声源过程噪声、测量噪声的统计特性输出不仅包含状态估计值还包含估计的不确定性协方差适用前提系统模型为线性可用扩展卡尔曼滤波EKF处理非线性噪声服从高斯分布零均值、白噪声系统状态可观测或至少可检测二、卡尔曼滤波的数学模型1、状态方程与观测方程离散时间系统嵌入式常用x k F k x k − 1 B k u k w k z k H k x k v k \begin{aligned} \mathbf{x}_k \mathbf{F}_k \mathbf{x}_{k-1} \mathbf{B}_k \mathbf{u}_k \mathbf{w}_k \\ \mathbf{z}_k \mathbf{H}_k \mathbf{x}_k \mathbf{v}_k \end{aligned}xkzkFkxk−1BkukwkHkxkvk其中w k ∼ N ( 0 , Q k ) \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k)wk∼N(0,Qk)v k ∼ N ( 0 , R k ) \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k)vk∼N(0,Rk)。2、卡尔曼滤波两步法五大方程预测(时间更新)x ^ k ∣ k − 1 F k x ^ k − 1 ∣ k − 1 B k u k P k ∣ k − 1 F k P k − 1 ∣ k − 1 F k ⊤ Q k \begin{aligned} \hat{\mathbf{x}}_{k|k-1} \mathbf{F}_k \hat{\mathbf{x}}_{k-1|k-1} \mathbf{B}_k \mathbf{u}_k \\ \mathbf{P}_{k|k-1} \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top \mathbf{Q}_k \end{aligned}x^k∣k−1Pk∣k−1Fkx^k−1∣k−1BkukFkPk−1∣k−1Fk⊤Qk更新(测量更新)K k P k ∣ k − 1 H k ⊤ ( H k P k ∣ k − 1 H k ⊤ R k ) − 1 x ^ k ∣ k x ^ k ∣ k − 1 K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k ( I − K k H k ) P k ∣ k − 1 \begin{aligned} \mathbf{K}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \left( \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{R}_k \right)^{-1} \\ \hat{\mathbf{x}}_{k|k} \hat{\mathbf{x}}_{k|k-1} \mathbf{K}_k \left( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \right) \\ \mathbf{P}_{k|k} \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_{k|k-1} \end{aligned}Kkx^k∣kPk∣kPk∣k−1Hk⊤(HkPk∣k−1Hk⊤Rk)−1x^k∣k−1Kk(zk−Hkx^k∣k−1)(I−KkHk)Pk∣k−1
卡尔曼滤波（状态估计算法）
文章目录一、卡尔曼滤波的本质与适用场景核心特点适用前提二、卡尔曼滤波的数学模型1、状态方程与观测方程2、卡尔曼滤波两步法五大方程预测(时间更新)更新(测量更新)卡尔曼滤波Kalman Filter是嵌入式开发中极为核心的状态估计算法尤其适用于存在噪声的传感器数据融合和动态系统状态估计。一、卡尔曼滤波的本质与适用场景卡尔曼滤波是一种最优递归数据融合算法在线性高斯假设下通过预测系统模型与更新测量两个步骤不断迭代得到系统状态的最小均方误差估计。核心特点递推计算无需存储全部历史数据适合嵌入式实时系统融合多个噪声源过程噪声、测量噪声的统计特性输出不仅包含状态估计值还包含估计的不确定性协方差适用前提系统模型为线性可用扩展卡尔曼滤波EKF处理非线性噪声服从高斯分布零均值、白噪声系统状态可观测或至少可检测二、卡尔曼滤波的数学模型1、状态方程与观测方程离散时间系统嵌入式常用x k F k x k − 1 B k u k w k z k H k x k v k \begin{aligned} \mathbf{x}_k \mathbf{F}_k \mathbf{x}_{k-1} \mathbf{B}_k \mathbf{u}_k \mathbf{w}_k \\ \mathbf{z}_k \mathbf{H}_k \mathbf{x}_k \mathbf{v}_k \end{aligned}xkzkFkxk−1BkukwkHkxkvk其中w k ∼ N ( 0 , Q k ) \mathbf{w}_k \sim \mathcal{N}(0, \mathbf{Q}_k)wk∼N(0,Qk)v k ∼ N ( 0 , R k ) \mathbf{v}_k \sim \mathcal{N}(0, \mathbf{R}_k)vk∼N(0,Rk)。2、卡尔曼滤波两步法五大方程预测(时间更新)x ^ k ∣ k − 1 F k x ^ k − 1 ∣ k − 1 B k u k P k ∣ k − 1 F k P k − 1 ∣ k − 1 F k ⊤ Q k \begin{aligned} \hat{\mathbf{x}}_{k|k-1} \mathbf{F}_k \hat{\mathbf{x}}_{k-1|k-1} \mathbf{B}_k \mathbf{u}_k \\ \mathbf{P}_{k|k-1} \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^\top \mathbf{Q}_k \end{aligned}x^k∣k−1Pk∣k−1Fkx^k−1∣k−1BkukFkPk−1∣k−1Fk⊤Qk更新(测量更新)K k P k ∣ k − 1 H k ⊤ ( H k P k ∣ k − 1 H k ⊤ R k ) − 1 x ^ k ∣ k x ^ k ∣ k − 1 K k ( z k − H k x ^ k ∣ k − 1 ) P k ∣ k ( I − K k H k ) P k ∣ k − 1 \begin{aligned} \mathbf{K}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \left( \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^\top \mathbf{R}_k \right)^{-1} \\ \hat{\mathbf{x}}_{k|k} \hat{\mathbf{x}}_{k|k-1} \mathbf{K}_k \left( \mathbf{z}_k - \mathbf{H}_k \hat{\mathbf{x}}_{k|k-1} \right) \\ \mathbf{P}_{k|k} \left( \mathbf{I} - \mathbf{K}_k \mathbf{H}_k \right) \mathbf{P}_{k|k-1} \end{aligned}Kkx^k∣kPk∣kPk∣k−1Hk⊤(HkPk∣k−1Hk⊤Rk)−1x^k∣k−1Kk(zk−Hkx^k∣k−1)(I−KkHk)Pk∣k−1
