卡尔曼滤波

CHD_Apple

于 2024-06-27 18:06:54 发布

阅读量204

点赞数 3

分类专栏：数据结构与算法补充知识智能驾驶文章标签：算法开发语言

本文链接：https://blog.csdn.net/chd_apple/article/details/140020202

版权

智能驾驶同时被 3 个专栏收录

18 篇文章 5 订阅

订阅专栏

补充知识

8 篇文章 0 订阅

订阅专栏

数据结构与算法

7 篇文章 0 订阅

订阅专栏

基于B站DR-CAN举的excel例子，这里采用matlab将其实现

1、kalman 核心5个公式

举的例子

2、卡尔曼例子基于DR_CAN的例子

clc;
clear all;

%%%%%kalman filter%%%%%%%%%%%%%

%生成一段时间
t = 1:0.5:30;
L = length(t);

%实际信号与测量信号
n = 2;
X = zeros(n,L);
Z = zeros(n,L);
X(:,1) = [0,1]';
A = [1,1;0,1];
W =normrnd(0,1,2,L);
H = [1,0;0,1];
V = normrnd(0,1,2,L);
for i = 2:L
    X(:,i) = A*X(:,i-1)+W(:,i-1);
    Z(:,i) = H*X(:,i) + V(:,i);
end
%画出测量信号与实际信号
plot(t,X(1,:),'g','LineWidth',1)
hold on
plot(t,Z(1,:),'r','LineWidth',1)
legend('实际信号','测量信号');

%%%%%滤波算法%%%%%%%%%%%%%
%矩阵初始化
X_hat_min = zeros(2,L);
X_min = zeros(2,L);
P_min = zeros(2,2);
P = [0.1,0;0,0.1];%初始的P0
Q = [0.01,0;0,0.01];
R = [0,0.01;0,0.01];
K = zeros(2,2);
X_hat = zeros(2,L);
X_hat(:,1) = [0,1];%初始的X0
I = eye(2);
for k = 2:L
    %预测
    X_hat_min(:,k) = A*X_hat(:,k-1);
    P_min = A*P*A' + Q;
    %校正
    K = (P_min*H')*inv((H*P_min*H' +R));
    X_hat(:,k) = X_hat_min(:,k) + K*(Z(:,k) - H*X_hat_min(:,k));
    P = (I - K*H)*P_min;
end

figure
plot(t,X(1,:),'g')
hold on
plot(t,Z(1,:),'r')
hold on 
plot(t,X_hat(1,:),'b','linewidth',0.5)
legend('实际信号','测量信号','滤波后位置')
%%%%%%%%%%%%%%%%%%%%%%
figure
plot(t,X(2,:),'g')
hold on
plot(t,Z(2,:),'r')
hold on 
plot(t,X_hat(2,:),'b','linewidth',0.5)
legend('实际信号','测量信号','滤波后速度');