实际工程项目中是怎么用卡尔曼滤波的?
共 15016字，需浏览 31分钟
· 2024-04-19
点击上方“小白学视觉”，选择加"星标"或“置顶”
重磅干货，第一时间送达
编辑 | 汽车人
原文链接：https://www.zhihu.com/question/358334095
回答一
作者：李崇
链接：https://www.zhihu.com/question/358334095/answer/1160183841
两大难题，一是运动学模型的建立，也就是预测方程。这个一方面可以通过比较细的系统辨识来做，这方面要结合具体的应用背景，不一而足，也是普遍做的比较差的地方。
另一方面可用某个传感器的输出来做预测，比较典型的就是用加速度计做预测，再用陀螺做更新。
另一大问题就是噪声的统计特性咯。因为建模误差不知道。观测方程到还可以测一下ZRO算一下均值和方差。然后下一步就是大家喜闻乐见的调参的阶段咯，据说是群魔乱舞花样百出。
我现实中只用过mems加速度计和陀螺的融合，还有就是无模型的跟踪咯。也就是预测方程是0，纯靠建模误差的那个方差来做预测，效果咋说呢，稳态下看噪声统计特性是好了些，但是动态特性明显滞后和变形了。应该是我经验不足吧
回答二
作者：雁集
链接：https://www.zhihu.com/question/358334095/answer/1009240518
先啰嗦一下在工程中卡尔曼滤波要解决的问题是什么。
我们得到了2（或多个）个反馈同一件事的信息A和B，但不知道该相信哪个结果，所以简单的，将两个信息相加除以2（加权平均），得到一个结果0.5A+0.5B。但这个方法过于粗暴，不能反映出水平。现在知道了其中一个信息可靠度更好，而另一个可靠度不好，就要更相信好的信息，如，现在给可信度好的B更大的权重，得到0.2A+0.8B，这个结果会好于第一个。但仍然觉得low，因为A与B的参数是根据经验设置的，并不知道是否是最合理的，是否是最优化的。所以高级的，用优化的方法，根据A与B的高斯分布特征，去融合A和B信息，这个就是卡尔曼。
所以，我们看到，要利用卡尔曼，需要不同的信息源（大于等于2个）。最常见的信息源是什么？就是我们根据系统模型或动力学模型计算出来的结果，可以通过仿真的形式得到。第二个或第三个信息是什么？在飞行器姿态估计，导航定位，目标跟踪（几个问题的本质一样，都是状态估计）的问题中，这个信息一般来自惯导或GPS或雷达等。有了2个信息数据，就可以根据Kalman的公式进行融合。下面举一个机器人运动估计的例子，来自《Probabilistic Robotics》。
首先假设你的机器人长这样（一个圆，比较简单），在二维笛卡尔坐标系中运动。
根据这个图，可以得到它的运动学模型
得到以上模型还不能直接用，这个模型有变量的乘积和三角函数，是个非线性模型，但要利用卡尔曼公式，需要满足EKF（实际工程应用中，我还没见过直接用KF的）的线性化条件。所以要将非线性的模型线性化。
到这一步，得到的Gt可以直接代入EKF公式去更新协方差矩阵了，经过推算会得到增益矩阵K。EKF公式包括预测与更新，这里就不啰嗦了。
同时，用状态的迭代形式，可以得到初步推算的位置。
最后，EKF公式中有一项
其中两个z，代表了2个信息，比如其中一个是通过动力学得到的机器人位置，另一个通过雷达得到的机器人位置。
通过以上的计算，会得到一个靠谱的结果。
Kalman的其它公式都是用优化理论去解算高斯函数的，在此不啰嗦了。
回答三
作者：虚函数机器人
链接：https://www.zhihu.com/question/358334095/answer/2396600132
关于卡尔曼原理的讲解知乎上有太多了，而且理解的都比我好，在这里我就不班门弄斧了。我实现了一个融合惯导的加速度计和陀螺仪的简易卡尔曼滤波器，代码写的十分简单，保证初学者一看就懂，对那些复现算法有困难的同学会起到一定帮助。话不多说，上干货： 会用到的卡尔曼脑仁疼公式如下：
加速度计数学模型满足：
代码融合如下，代码风格不好，各位别学我。（写这个代码的时候我还未成年）：
#include "imu.h"
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Dense>
#include<string>
#include<vector>
#include <cmath>
#include <limits>//用于生成随机分布数列
#include <stdlib.h>
#include <random>
#include <fstream>
#include <cstdlib> // 标准库
using namespace std;
using namespace Eigen;
using Eigen::MatrixXd;
int t=1;
double theta_PI=57.29578;
/* 定义系数*/
MatrixXd A(2,2);
MatrixXd B(2,1);
MatrixXd Qk(2,2);
MatrixXd H(1,2);
MatrixXd R(1,1);
MatrixXd I=MatrixXd::Identity(2,2);
Eigen::MatrixXd angle_1(1,1);
vector<MatrixXd> X_k;MatrixXd Xk=MatrixXd::Constant(2,1,0);
vector<MatrixXd> X_k_1;MatrixXd Xk_1=MatrixXd::Constant(2,1,0);
vector<MatrixXd> P_k;MatrixXd Pk=MatrixXd::Constant(2,2,0);
vector<MatrixXd> P_k_1;MatrixXd Pk_1=MatrixXd::Constant(2,2,0);
vector<MatrixXd> K_K;MatrixXd K=MatrixXd::Constant(2,2,0);
vector<MatrixXd> Y_K;MatrixXd Yk=MatrixXd::Constant(1,1,0);
class Kalman
{
public:
Kalman()
{
IMU imu;
//A<<1,imu.dt,0,1;
//B<<imu.dt,0;
Qk<<0.03,0,0,0.003;//QK的值暂定一个常量，后续需要求解角度方差和角速度偏差方差
H(0,0)=1;H(0,1)=0;
R<<3;//测量噪声,暂时给一个定值
X_k.push_back(Xk);
X_k_1.push_back(Xk_1);
P_k.push_back(Pk);//此时刻的PK
P_k_1.push_back(Pk_1);//上一时刻的PK
K_K.push_back(K);
Y_K.push_back(Yk);
imu_sub=nd.subscribe<sensor_msgs::Imu>("imu", 100, &Kalman::IMUHandle,this);
position_pub=nd.advertise<sensor_msgs::Imu>("imu_position",1000);
}
void IMUHandle(const sensor_msgs::Imu::ConstPtr &imu_measure)
{
IMU Tx;
Tx.angle_v_x=imu_measure->angular_velocity.x;
Tx.angle_v_y=imu_measure->angular_velocity.y;
Tx.angle_v_z=imu_measure->angular_velocity.z;
Tx.acc_x=imu_measure->linear_acceleration.x;
Tx.acc_y=imu_measure->linear_acceleration.y;
Tx.acc_z=imu_measure->linear_acceleration.z;
double angle=-atan2(Tx.acc_x,sqrt(Tx.acc_y*Tx.acc_y+Tx.acc_z*Tx.acc_z));
double angle_1=angle*theta_PI;
double angle_v=Tx.angle_v_x;
// cout<<Tx.acc_x<<" "<<Tx.acc_y<<" "<<Tx.acc_z<<" "<<angle_1<<endl;
kalman_angle_x(angle,angle_v);
}
void kalman_angle_x(double angle,double angle_V)//angle是加速度计算出的角度 ,angle_V 角速度
{
IMU imu;
angle_1(0,0)=angle;
//卡尔曼第1个公式
Xk_1(0,0)=(angle_V-imu.bias)*(t-1)*0.04;
Xk_1(1,0)=imu.bias;
X_k_1[0](0,0)=Xk_1(0,0);
X_k_1[0](1,0)=Xk_1(1,0);
A<<1,imu.dt*t,0,1;
B<<imu.dt*t,0;
Xk=A*X_k_1[t-1]+B*angle_V;
X_k.push_back(Xk);
//卡尔曼第2个公式
Pk=A*P_k_1[t-1]*A.transpose()+Qk;
P_k.push_back(Pk);
//卡尔曼第3个公式
MatrixXd tmp(1,1);
tmp=H*P_k[t]*H.transpose()+R;
K=P_k[t]*H.transpose()*tmp.inverse();
K_K.push_back(K);
Yk=angle_1-H*X_k[t];
Y_K.push_back(Yk);
//卡尔曼第4个公式
Xk_1=X_k[t]+K_K[t]*Y_K[t];
X_k_1.push_back(Xk_1);
//卡尔曼第5个公式
Pk=(I-K_K[t]*H)*P_k[t];
P_k_1.push_back(Pk);
cout<<X_k[t](0,0)-angle<<endl;
// cout<<angle<<endl;
t++;
}
private:
ros::NodeHandle nd;
ros::Publisher position_pub;
ros::Subscriber imu_sub;
};
int main(int argc, char** argv)
{
ros::init(argc, argv,"kalman");
Kalman PublicIMU;
ros::spin();
return 0;
}
完整的代码在我的gitee：
https://gitee.com/angry-potato/imu-kalman
效果展示在这里：
纵坐标是观测值和预测值之间的error，数据输入全部都是来源于惯导，当我快速抖动惯导之后，就有了上面的波浪部分，但是很快error又趋近于0。
什么？？？？？你说你看不懂C++？
我这里也有份MATLAB的代码，可以参考一下，不过是网上的：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%程序功能： 使用扩展卡尔曼滤波器(EKF)估计平抛物体的运动
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;
kx = .01; ky = .05; % 阻尼系数
g = 9.8; % 重力
t = 10; % 仿真时间
Ts = 0.1; % 采样周期
len = fix(t/Ts); % 仿真步数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%（真实轨迹模拟）
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
x = x + vx*Ts;
vx = vx + (-kx*vx^2+dax*randn(1,1))*Ts;
y = y + vy*Ts;
vy = vy + (ky*vy^2-g+day*randn(1))*Ts;
X(k,:) = [x, vx, y, vy];
end
figure(1), hold off, plot(X(:,1),X(:,3),'-b'), grid on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 构造量测量
mrad = 0.001;
dr = 10; dafa = 10*mrad; % 量测噪声
for k=1:len
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
a = atan(X(k,1)/X(k,3)) + dafa*randn(1,1);
Z(k,:) = [r, a];
end
figure(1), hold on, plot(Z(:,1).*sin(Z(:,2)), Z(:,1).*cos(Z(:,2)),'*')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ekf 滤波
Qk = diag([0; dax; 0; day])^2;
Rk = diag([dr; dafa])^2;
Xk = zeros(4,1);
Pk = 100*eye(4);
X_est = X;
for k=1:len
Ft = JacobianF(X(k,:), kx, ky, g);
Hk = JacobianH(X(k,:));
fX = fff(X(k,:), kx, ky, g, Ts);
hfX = hhh(fX, Ts);
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
end
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
xlabel('X'); ylabel('Y'); title('ekf simulation');
legend('real', 'measurement', 'ekf estimated');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function F = JacobianF(X, kx, ky, g) % 系统状态雅可比函数
vx = X(2); vy = X(4);
F = zeros(4,4);
F(1,2) = 1;
F(2,2) = -2*kx*vx;
F(3,4) = 1;
F(4,4) = 2*ky*vy;
end
function H = JacobianH(X) % 量测雅可比函数
x = X(1); y = X(3);
H = zeros(2,4);
r = sqrt(x^2+y^2);
H(1,1) = 1/r; H(1,3) = 1/r;
xy2 = 1+(x/y)^2;
H(2,1) = 1/xy2*1/y; H(2,3) = 1/xy2*x*(-1/y^2);
end
function fX = fff(X, kx, ky, g, Ts) % 系统状态非线性函数
x = X(1); vx = X(2); y = X(3); vy = X(4);
x1 = x + vx*Ts;
vx1 = vx + (-kx*vx^2)*Ts;
y1 = y + vy*Ts;
vy1 = vy + (ky*vy^2-g)*Ts;
fX = [x1; vx1; y1; vy1];
end
function hfX = hhh(fX, Ts) % 量测非线性函数
x = fX(1); y = fX(3);
r = sqrt(x^2+y^2);
a = atan(x/y);
hfX = [r; a];
end
function [Xk, Pk, Kk] = ekf(Phikk_1, Qk, fXk_1, Pk_1, Hk, Rk, Zk_hfX) % ekf 滤波函数
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk; Kk = Pxz*Pzz^-1;
Xk = fXk_1 + Kk*Zk_hfX;
Pk = Pkk_1 - Kk*Pzz*Kk'
end以上。
下载1：OpenCV-Contrib扩展模块中文版教程
在「小白学视觉」公众号后台回复：扩展模块中文教程，即可下载全网第一份OpenCV扩展模块教程中文版，涵盖扩展模块安装、SFM算法、立体视觉、目标跟踪、生物视觉、超分辨率处理等二十多章内容。
下载2：Python视觉实战项目52讲
在「小白学视觉」公众号后台回复：Python视觉实战项目，即可下载包括图像分割、口罩检测、车道线检测、车辆计数、添加眼线、车牌识别、字符识别、情绪检测、文本内容提取、面部识别等31个视觉实战项目，助力快速学校计算机视觉。
下载3：OpenCV实战项目20讲
在「小白学视觉」公众号后台回复：OpenCV实战项目20讲，即可下载含有20个基于OpenCV实现20个实战项目，实现OpenCV学习进阶。
交流群
欢迎加入公众号读者群一起和同行交流，目前有SLAM、三维视觉、传感器、自动驾驶、计算摄影、检测、分割、识别、医学影像、GAN、算法竞赛等微信群（以后会逐渐细分），请扫描下面微信号加群，备注：”昵称+学校/公司+研究方向“，例如：”张三 + 上海交大 + 视觉SLAM“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进入相关微信群。请勿在群内发送广告，否则会请出群，谢谢理解~