编辑 | 汽车人
原文链接:https://www.zhihu.com/question/358334095
点击下方卡片,关注“自动驾驶之心”公众号
ADAS巨卷干货,即可获取
点击进入→自动驾驶之心【目标跟踪】技术交流群
后台回复【卡尔曼滤波】获取卡尔马滤波学习资料!!!
回答一
作者:李崇
链接: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》。
首先假设你的机器人长这样(一个圆,比较简单),在二维笛卡尔坐标系中运动。
![e245a06260652a68331b8e6c2da9c63e.png](https://i-blog.csdnimg.cn/blog_migrate/6cb5f2cce20b20c483154f8e459eb472.png)
根据这个图,可以得到它的运动学模型
![917b201067ed682a1af441c6154eb611.png](https://i-blog.csdnimg.cn/blog_migrate/904e6b0b4ba3ed973db7259f78d6d5c1.png)
得到以上模型还不能直接用,这个模型有变量的乘积和三角函数,是个非线性模型,但要利用卡尔曼公式,需要满足EKF(实际工程应用中,我还没见过直接用KF的)的线性化条件。所以要将非线性的模型线性化。
![d78ae8e67a59e3b4a6767ad6264c6219.png](https://i-blog.csdnimg.cn/blog_migrate/8391b646d7eedfd256820b19080d85f8.png)
![5aab2a6d52a18cc964f4091a37d3f613.png](https://i-blog.csdnimg.cn/blog_migrate/9c3eef9dfdde23d3e8727776874fecb5.png)
![73d9c4d80199bdaae9d1e379bc150aeb.png](https://i-blog.csdnimg.cn/blog_migrate/71bda4e7699d7b4e23510f5a7133d214.png)
到这一步,得到的Gt可以直接代入EKF公式去更新协方差矩阵了,经过推算会得到增益矩阵K。EKF公式包括预测与更新,这里就不啰嗦了。
同时,用状态的迭代形式,可以得到初步推算的位置。
![210098c066065b76c2e6728beb4c490b.png](https://i-blog.csdnimg.cn/blog_migrate/e819f89eb5c31091669b9fa80660181c.png)
最后,EKF公式中有一项
![e33ea863aaaf6dc925edf7aa4b916d61.png](https://i-blog.csdnimg.cn/blog_migrate/6b2fab01f87e026e4699708be5f7b2c5.png)
其中两个z,代表了2个信息,比如其中一个是通过动力学得到的机器人位置,另一个通过雷达得到的机器人位置。
通过以上的计算,会得到一个靠谱的结果。
Kalman的其它公式都是用优化理论去解算高斯函数的,在此不啰嗦了。
回答三
作者:虚函数机器人
链接:https://www.zhihu.com/question/358334095/answer/2396600132
关于卡尔曼原理的讲解知乎上有太多了,而且理解的都比我好,在这里我就不班门弄斧了。我实现了一个融合惯导的加速度计和陀螺仪的简易卡尔曼滤波器,代码写的十分简单,保证初学者一看就懂,对那些复现算法有困难的同学会起到一定帮助。话不多说,上干货: 会用到的卡尔曼脑仁疼公式如下:
![63f05be059a2c653913ea71b69650af0.png](https://i-blog.csdnimg.cn/blog_migrate/95a9dddd8f8a1268679cf1071afc61ab.png)
加速度计数学模型满足:
![fa19201d38c5f391354ce65f1928fb0c.png](https://i-blog.csdnimg.cn/blog_migrate/1a3f751b5604e3e512f91a897ac825fc.png)
代码融合如下,代码风格不好,各位别学我。(写这个代码的时候我还未成年):
#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
效果展示在这里:
![d0036cec991aa6ba31345dc6963d146b.png](https://i-blog.csdnimg.cn/blog_migrate/74f42f374615ff944ec355f693ae61a7.png)
纵坐标是观测值和预测值之间的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以上。
请点个赞再走吧,谢谢各位老爷了!
【自动驾驶之心】全栈技术交流群
自动驾驶之心是首个自动驾驶开发者社区,聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、硬件配置、AI求职交流等方向;
添加汽车人助理微信邀请入群
备注:学校/公司+方向+昵称