详解卡尔曼滤波算法、平均平滑滤波算法（原理+实现）
实验介绍
卡尔曼滤波算法是一种通过观测值和先验知识预测未来状态的算法，最初由美国工程师卡尔曼提出，应用广泛于控制系统、信号处理、机器人、航空航天和金融等领域。卡尔曼滤波算法通常被用来估计某个系统的状态，例如移动机器人的位置和速度，飞行器的姿态和位置等。
卡尔曼滤波器的基本框架主要包括两个步骤，即预测和更新。预测步骤根据上一个时刻的状态估计当前时刻的状态，并预测当前时刻的观测值；更新步骤接收实际观测值，对预测值进行修正，得到更准确的状态估计。
知识点
• 卡尔曼滤波算法
• 平均平滑滤波算法
卡尔曼滤波器主要需要以下信息
1. 系统模型：包括状态方程和观测方程。状态方程描述系统的状态和状态之间的转移关系，观测方程描述系统的状态和观测值之间的关系。
2. 状态估计：包括状态向量和协方差矩阵。状态向量是系统状态的估计值，协方差矩阵衡量估计值的精确度。
3. 观测值：包括测量向量和测量噪声协方差矩阵。测量向量是实际观测到的值，测量噪声协方差矩阵描述观测值的噪声情况。
在预测步骤中，卡尔曼滤波器首先根据上一个时刻的状态和状态转移矩阵，预测当前时刻的状态。同时还需要根据上一个时刻的协方差矩阵和系统噪声协方差矩阵，计算当前时刻的协方差矩阵。在更新步骤中，卡尔曼滤波器根据测量向量和观测噪声协方差矩阵，计算测量残差并估计当前状态向量的最优值和协方差矩阵。
总之，卡尔曼滤波器是一种基于状态估计的算法，通过观测值和先验知识预测未来状态，并在接收实际观测值时对预测值进行修正，得到更准确的状态估计，具有很强的适用性和鲁棒性。
卡尔曼滤波算法数学原理
卡尔曼滤波算法的数学原理可以用以下公式表示：
1. 预测步骤：
• 状态预测方程： x_k = F * x_{k-1} + B * u_k + w_k
• 协方差预测方程： P_k = F * P_{k-1} * F^T + Q
其中，
• x_k 是当前时刻的状态向量；
• F 是状态转移矩阵，描述状态从上一个时刻到当前时刻的转移关系；
• x_{k-1} 是上一个时刻的状态向量；
• B 是控制输入矩阵，描述外部控制对系统的影响；
• u_k 是当前时刻的控制输入向量；
• w_k 是状态转移噪声，服从零均值的高斯分布，协方差为Q；
• P_k 是当前时刻的状态协方差矩阵；
• P_{k-1} 是上一个时刻的状态协方差矩阵。
2. 更新步骤：
• 测量残差（创新）： y_k = z_k - H * x_k
• 测量残差协方差： S_k = H * P_k * H^T + R
• 卡尔曼增益： K_k = P_k * H^T * S_k^-1
• 更新状态估计： x_k = x_k + K_k * y_k
• 更新协方差估计： P_k = (I - K_k * H) * P_k
其中，
• z_k 是当前时刻的观测值向量；
• H 是观测矩阵，描述观测值和状态的关系；
• R 是观测噪声协方差矩阵，服从零均值的高斯分布；
• y_k 是测量残差（创新）向量；
• S_k 是测量残差协方差矩阵；
• K_k 是卡尔曼增益矩阵；
• I 是单位矩阵。
以上公式描述了卡尔曼滤波算法在预测步骤和更新步骤中的数学运算。通过递归地进行状态预测和状态更新，卡尔曼滤波器能够实现对系统状态的最优估计。需要注意的是，上述公式中的矩阵和向量的维度要与具体问题相符合。
实现卡尔曼滤波
function [result,x_now,p_now]= kalmanFilter(kalman_val, A, Q, R)
if A(1,1) == 0 && A(1,2) == 0
A(1,1) = kalman_val;
A(1,2) = kalman_val;
end
x_mid = A(1,1);
p_mid = A(1,2)+Q;
kg= p_mid/(p_mid+R);
x_now = x_mid + kg*(kalman_val - x_mid);
p_now = (1-kg)*p_mid;
A(1,1) = x_now;
A(1,2) = p_now;
result = x_now;
endlegend('原始数据','滤波的数据')
clear
clc
%假如原始数据1到3m
x = 1:100;
data = 2.*rand(1,100)+1;
result_data = [];
R=0.542;
Q=0.018;
A = [0,0];
for i = 1:100
[result_1,x_now,p_now] = kalmanFilter(data(1,i), A, Q, R);
A(1,1) = x_now;
A(1,2) = p_now;
result_data = [result_data, result_1];
end
plot(x,data,'-o',x,result_data,'-x')
title('卡尔曼滤波')
ylabel('单位m')
平均平滑滤波算法
平均平滑滤波（Moving Average Smoothing）是一种简单的信号平滑方法，用于减少噪声对信号的影响、降低信号的变动性，并提供更稳定的估计。该方法通过计算一段时间内的观测值的平均来平滑信号。
平均平滑滤波的数学原理如下：
给定一个长度为N的观测值序列 X = [x_1, x_2, ..., x_N]，其中 x_i 表示第i个观测值。
选择一个窗口大小 M，即每次计算的观测值个数。例如，选择 M=3，表示每次计算三个连续的观测值的平均。
应用平均平滑滤波的步骤如下：
1. 初始化：
• 选择窗口大小 M。
• 设置滑动窗口的初始位置为 i=0。
2. 滑动窗口平均值计算：
• 对于每个位置 i，计算窗口内连续 M 个观测值的平均值： y_i = (1/M) * (x_i + x_{i+1} + ... + x_{i+M-1})
3. 更新滑动窗口位置：
• 将滑动窗口向右移动一个位置：i = i + 1。
4. 重复步骤 2 和步骤 3，直到滑动窗口无法再向右移动。
最终得到平滑后的序列 Y = [y_1, y_2, ..., y_{N-M+1}]，其中 y_i 是每个滑动窗口内观测值的平均。
平均平滑滤波方法简单易懂，适用于平稳的信号，并且可以有效地降低高频噪声的影响。然而，它可能会引入一定的延迟，并且对于快速变化的信号和非平稳的信号效果有限。在选择窗口大小时需要考虑权衡平滑程度和对信号的响应速度。
实现平均平滑滤波算法
假设有一变量在1到3m的变化区间，通过平均平滑滤波对结果进行过滤处理。
clear
clc
%假如原始数据1到3m
x = 1:100;
data = 2.*rand(1,100)+1
smoothed_data = smoothdata(data, 'movmean',50);
plot(x,data,'-o',x,smoothed_data,'-x')
title('移动平均值滤波')
ylabel('单位m')
legend('原始数据','滤波的数据')原始数据会遇到数据突变现象，可以添加对应的噪声数据。观察过滤后的效果。
legend('原始数据','滤波的数据')
clear
clc
%假如原始数据1到3m
x = 1:100;
data = 2.*rand(1,100)+1
data(1,20) = 7;
data(1,40) = 8;
data(1,70) = 5.5;
smoothed_data = smoothdata(data, 'movmean',50);
plot(x,data,'-o',x,smoothed_data,'-x')
title('移动平均值滤波')
ylabel('单位m')
实验总结
• 卡尔曼滤波： 在动态系统中具有较好的实时性能，能够有效地融合过程和观测噪声，提高状态估计的准确性。 对于线性系统表现良好，但在非线性系统中可能需要扩展卡尔曼滤波或其他非线性滤波方法。
• 平均平滑滤波： 实现简单且计算量小，适合处理稳定且波动较小的数据。对于突发噪声敏感，可能导致重要信号的丢失。
