卡尔曼滤波实战

入门内容

假如有个超声波，它传回的数据是这样的，这样的数据是用不了的

我们想要的是稳定的数据

此时我们引入滤波，把里面的噪声去掉，使得数据更平滑

适用系统：

符合两个特质：线性和高斯。也叫线性高斯系统

什么是线性？

满足两个特性：叠加性和齐次性

叠加型：

假如有两个输入，最后得到一个输出

等价于，总输出是y，由另外两个输出合起来，两个输出对应的输入是x1和x2

那么从方程理解

齐次性：

x增大k倍，y也增大k倍

高斯：

噪声满足正态分布

正态分布就是假如均值为零，它的方差对应着sigma

从宏观来看，卡尔曼滤波就是加权

理想状态：信号 x 1 + 噪声 x 0

从信号处理的角度来看，类比于低通滤波，低频信号权值为1，高频噪声取值为0

卡尔曼滤波就是有一个估计值和观测值，让你选择它们的权重，融合在一起就是最佳的修正值

进阶内容

状态空间表达式

状态方程

x是状态值，k是当前，k-1是上一个时刻状态，u为输入，w是过程造成（也叫误差）

观测方程

y是观察量，v是观测噪声（传感器自身的误差）

高斯分布

均值和方差

参数分析

主要就是调Q和R

卡尔曼直观图解

最优估计值也叫修正值或者后验估计值

这个是上一时刻的最优估计值，就是卡尔曼滤波最终输出的值

先验估计值

这个值是基于上一时刻的最优估计值估计出来的一个当前估计值

当前时刻的观测值，也就是传感器直接测量出来的值

缺一个值，就是当前时刻的最优估计值。大概在这里

滤波次数过多的时候就会趋于一个稳定状态，它的方差自然会越来越小好

当前估计值与当前观测值的交集

卡尔曼公式理解：

实现过程：用上一次的最优结果预测当前的值

同时使用观测值修正当前值，得到最优结果

python 复制代码

import numpy as np

def kalman_filter(observed_data, initial_state_estimate, initial_covariance_estimate, transition_matrix, observation_matrix, process_noise_covariance, observation_noise_covariance):
    # 初始化变量
    state_estimate = initial_state_estimate  # 状态估计
    covariance_estimate = initial_covariance_estimate  # 协方差估计
    
    # 遍历观测数据
    for observation in observed_data:
        # 预测步骤
        predicted_state_estimate = np.dot(transition_matrix, state_estimate)  # 预测状态估计
        predicted_covariance_estimate = np.dot(np.dot(transition_matrix, covariance_estimate), transition_matrix.T) + process_noise_covariance  # 预测协方差估计
        
        # 更新步骤
        innovation = observation - np.dot(observation_matrix, predicted_state_estimate)  # 创新
        innovation_covariance = np.dot(np.dot(observation_matrix, predicted_covariance_estimate), observation_matrix.T) + observation_noise_covariance  # 创新协方差
        kalman_gain = np.dot(np.dot(predicted_covariance_estimate, observation_matrix.T), np.linalg.inv(innovation_covariance))  # 卡尔曼增益
        
        state_estimate = predicted_state_estimate + np.dot(kalman_gain, innovation)  # 更新状态估计
        covariance_estimate = np.dot((np.eye(len(state_estimate)) - np.dot(kalman_gain, observation_matrix)), predicted_covariance_estimate)  # 更新协方差估计
    
    return state_estimate, covariance_estimate

# 示例用法
observed_data = np.array([1.2, 1.3, 1.4, 1.5])  # 观测测量
initial_state_estimate = np.array([1.0, 0.0])   # 初始状态猜测
initial_covariance_estimate = np.eye(2)         # 初始协方差猜测
transition_matrix = np.array([[1.0, 1.0], [0.0, 1.0]])  # 状态转移矩阵
observation_matrix = np.array([[1.0, 0.0]])      # 观测矩阵
process_noise_covariance = np.eye(2) * 0.01      # 过程噪声协方差
observation_noise_covariance = np.array([[0.1]]) # 观测噪声协方差

state_estimate, covariance_estimate = kalman_filter(observed_data, initial_state_estimate, initial_covariance_estimate, transition_matrix, observation_matrix, process_noise_covariance, observation_noise_covariance)

print("最终状态估计:", state_estimate)
print("最终协方差估计:", covariance_estimate)