1. Introducción al algoritmo
El filtrado de Kalman es un algoritmo de filtrado mágico muy utilizado, es un algoritmo de estimación de estado que combina experiencia previa y actualizaciones de medidas.
1. Estimación de estado
Primero, para una cantidad física que nos interesa, asumimos que se ajusta a la siguiente ley
Entre ellos, está el valor real de la cantidad física en este período, y es el valor real de la cantidad física en el período anterior. Por supuesto, esta cantidad física puede no cumplir con esta regla, y solo hicimos una suposición. Diferentes cantidades físicas se ajustan a diferentes leyes, según nuestra experiencia, de acuerdo con esta ley, podemos predecir las cantidades físicas que nos interesan. Por ejemplo, la cantidad física que nos importa es la velocidad del vehículo, si el vehículo se mueve a una velocidad constante, el valor de es 1, es decir, la velocidad de este ciclo es la misma que la del ciclo anterior.
下面我们再来看一下这个物理量的测量公式
donde es el valor medido de esta cantidad física y es el ruido de medición. Cuando predecimos una física, la medición es un medio indispensable. Aunque la medición no es necesariamente precisa, refleja en gran medida el valor real de la cantidad física. Esta fórmula refleja la relación entre el valor real y el valor medido. Aún tomando como ejemplo la velocidad del vehículo, es el valor medido obtenido por el sensor de velocidad del vehículo.
实际中，物理量一般不会像我们上面的公式那样简单，一般我们用下面的公式来表示
Entre ellos, uk representa el ruido de procesamiento, que es la diferencia entre el modelo de procesamiento y la situación real, como la velocidad del vehículo, que se verá afectada por factores externos como la aceleración artificial, la desaceleración y la superficie irregular de la carretera.
卡尔曼滤波的基本思想是综合利用上一次的状态和测量值来对物理量的状态进行预测估计。我们用来x^k表示Xk的估计值，则有下面的公式
En esta fórmula, el valor estimado del ciclo anterior y el valor medido de este ciclo se utilizan para estimar. Entre ellos, gk es la ganancia de Kalman. Esta fórmula es muy similar al filtro de primer orden, excepto que la ganancia de Kalman cambiará y se actualizará cada ciclo, y el coeficiente del filtro de primer orden es un valor fijo. Considere casos extremos para analizar el efecto de la ganancia, cuando gk = 0, la ganancia es 0. En este momento, esto significa que el valor estimado de nuestro ciclo es el mismo que el ciclo anterior, y el valor medido actual no es confiable ; cuando gk = 1 , la ganancia es 1, entonces x^k = x^k-1, lo que significa que nuestro valor estimado de este ciclo es el mismo que el valor medido, y no se confía en el valor estimado del ciclo anterior En aplicaciones prácticas, está entre 0 y 1, lo que representa el grado de confianza en el valor medido.
2. Ganancia de Kalman
Usamos la ganancia de Kalman para estimar el valor de la cantidad física anterior, entonces, ¿cómo obtiene su valor la ganancia de Kalman? Usamos las siguientes dos fórmulas para calcular y actualizar iterativamente cada ciclo.
3. Algoritmo de filtrado de Kalman completo
Con la derivación anterior, enumeramos a continuación la fórmula para completar el filtrado de Kalman. El filtrado de Kalman se divide en dos procesos: proceso de predicción y proceso de actualización. En la fórmula, introducimos el factor de escala y la covarianza.
上面的公式适合一维变量的卡尔曼滤波，将变量扩展到多维，用向量和矩阵替换上面的变量，就可以实现多维变量的卡尔曼滤波，下面的公式适用于多维变量。
Dos: código matlab (pro-test efectivo)
% ==================== 系统描述 ====================
% GPS精度为4m
% 加速度计的精度为 1cm/s^2，且有 3cm/s^2 的偏移
% 真实的加速度为 2cm/s^2
% 数据采集频率皆为 10Hz
% ================== 模拟产生数据 ==================
clear all
rng(0); % 设置随机数种子
dt = 0.1; % 0.1s采集一次数据
T = 0:dt:100; % 0~100s
N = length(T); % 采样点数
a = 2e-2; % 真实加速度
s = 1/2 * a * T.^2; % 真实位移（实际上不可知）
aa = a + 1e-2*randn(size(s)) + 3e-2; % 加速度计测量数据，有噪声，有偏移1
R = 4; % GPS 测量精度
z = s + sqrt(R)*randn(size(s)); % GPS 测量数据，开根号得到
% ================= 变量定义与初始化 =================
A = [1 dt; 0 1]; % 状态转移矩阵
H = [1 0]; % 转换矩阵
B = [1/2*dt^2; dt]; % 输入控制矩阵
Q = [0.001 0; 0 0.01]; % 过程噪声协方差，估计一个
%%
P = eye(2); % 初始值为 1（可为非零任意数）
%%
x = zeros(2, N); % 存储滤波后的数据，分别为位移、速度信息
k = 1; % 采样点计数
% ================= 卡尔曼滤波过程 ==================
for t = dt:dt:100
k = k + 1
x(:,k) = A * x(:,k-1) + B*aa(k); % 卡尔曼公式1
P = A * P * A' + Q; % 卡尔曼公式2
K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3
x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(2)-K*H) * P; % 卡尔曼公式5
end
% ==================== 结果绘图 =====================
figure(1);
plot(T, z(1,:),'b.');hold on; % GPS测量数据
plot(T, x(1,:),'r.'); % 滤波后数据
plot(T, s ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('距离: m');hold off;
axis([0 100 -20 120])
text(2, 110, ['滤波前方差: ' num2str(var(z - s))]) % 滤波前方差
text(2, 100, ['滤波后方差: ' num2str(var(x(1,:) - s))]) % 滤波后方差
figure(2);
plot(T(1:end-1), (x(1,2:end)-x(1,1:end-1))/dt, 'b.');hold on% 将滤波后位移差分结果
plot(T, x(2,:), 'r.'); % 滤波估计速度
plot(T, a*T, 'k-'); % 真实速度
legend('滤波位移差分','卡尔曼估计值','真实值')
xlabel('时间: s');
ylabel('速度: m');hold off;
axis([0 100 -8 10])
text(2, 9, ['位移差分方差: ' ...
num2str(var((x(1,2:end)-x(1,1:end-1))/dt - a*T(1:end-1)))]) % 滤波前方差
text(2, 7.6, ['速度滤波方差: ' ...
num2str(var(x(2,:) - a*T))]) % 滤波后方差
Cita:
https://blog.csdn.net/weixin_41869763/article/details/105792692
https://blog.csdn.net/bhniunan/article/details/104607668?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163970238816780366533676%2522%252C%2522scm%2522%2 53A%252220140713.130102334…%2522%257D&request_id= 163970238816780366533676&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2 all top_click~default-1-104607668.pc_search_insert_es_download_v2&utm_term=%E5%8D%A1%E5%B0%94%E6% 9B%BC%E6%BB%A4 %E6%B3%A2&spm=1018.2226.3001.4187