Kalman滤波处理运动小球圆心位置
用Kalman滤波处理自由落体运动小球圆心位置，效果图：








主要代码：

数据图：

需要代码和数据的 请加qq：398292861（备注来意） 谢谢

样例二是更加随机的情况，可见于博主的文章：matlab代码 画出不同位置高度的圆柱--样例二

圆柱信息：

圆心（1,1）高 1

圆心（3,1）高 2

圆心（4,3）高 3

clc
close
clear

%圆柱上表面自变量
[a,r]=ndgrid((0:.05:1)*2*pi,[0 1]);
xt=cos(a).*r;%21x2维
yt=sin(a).*r;%21x2维
%定义圆心
xx=[1;3;4];
yy=[1;1;3];
%圆柱高矩阵
zz=[1;2;3];
%中间变量
newxx=zeros(21,1);
newyy=zeros(21,1);
tempnewxx=newxx;
tempnewyy=newyy;
for j=1:3
    newxx=tempnewxx;
    newyy=tempnewyy;
    for i=1:21
        newxx(i,:)=xx(j,1);
        newyy(i,:)=yy(j,1);
    end
    %确定范围
    [a,z]=ndgrid((0:.05:1)*2*pi,0:.05:zz(j,1));
    x=cos(a);%21xsize(z,2)维 项数=（（末项-首项）/公差）+1,这里是(（1-0)/0.05)+1=21,z=1
    y=sin(a);%21xsize(z,2)维
    %画圆柱测面
    fuzhinewxx=newxx;
    fuzhinewyy=newyy;
    for i=1:(size(z,2)-1)
        newxx=cat(2,newxx,fuzhinewxx);
        newyy=cat(2,newyy,fuzhinewyy);
    end
    
    surf(x+newxx,y+newyy,z,x*0)
    axis equal
    hold on
    
    %画圆柱上表面
    newxx=fuzhinewxx;
    newyy=fuzhinewyy;
    for i=1:(2-1)
        newxx=cat(2,newxx,fuzhinewxx);
        newyy=cat(2,newyy,fuzhinewyy);
    end  
    surf(xt+newxx,yt+newyy,xt*0,xt*0)
    surf(xt+newxx,yt+newyy,xt*0+zz(j,1),xt*0)
end

结果：

展开全部
figure( 1 );
img = imread( 'D:\TDDOWNLOAD\Kindle Book\封面图片\23174533-1_u_1.jpg' );
imshow( img );
% 使用椭圆在figure上创建一个椭圆，这62616964757a686964616fe58685e5aeb931333335316537四个数决定你截图的位置和大小
% [ 椭圆外接矩形的左上角x， 左上角y， 宽，长 ]
% 这个椭圆在figure上可以拖拽
h = imellipse( gca, [ 150, 150, 300, 500 ] );
% 把这个椭圆转为一个和img同样大小的二值图，1表示椭圆内部，0表示外部
BW = createMask( h );
% 根据二值图构建截图，椭圆外的都染黑
[ w, h, d3 ] = size( img );
newimg = zeros( w, h, d3, 'uint8' );
for i = 1 : d3
band = img( :, :, i );
newband = zeros( w, h, 'uint8' );
newband( BW ) = band( BW );
newimg( :, :, i ) = newband;
end
figure( 2 );
imshow( newimg );
% imwrite( 'D:\newimg.jpg' );% 截图可以保存

参考链接：https://blog.csdn.net/weixin_44492796/article/details/88583536

绘制圆柱需要提供上底面和下底面的圆心的绝对坐标。以及半径。通过得到圆柱高的向量，然后通过null函数，得到正交基，获得相对坐标系，首先计算出圆柱相对坐标，之后通过坐标变换法获得圆柱两底面的绝对坐标。之后通过surf函数绘制。

%绘制圆柱
%需要知道中轴线线段的位置，以及圆筒的半径
%中轴线两端点的坐标，圆柱的高度。
obstracle_R=5;
obstracle_L_center=[10 70 0];
obstracle_H_center=[10 10 40];

 
%建立底面圆心所在的坐标系
Vector=obstracle_H_center-obstracle_L_center;
obstracle_hight=norm(Vector);
CZ=Vector/norm(Vector);
CZout=null(CZ);
CX=CZout(:,1);
CY=CZout(:,2);
CZ=CZ';
Trans=[[CX CY CZ obstracle_L_center'];0 0 0 1];
Lx=zeros(2,51);
Ly=Lx;
Lz=Lx;
 
for i=1:50
   Lx(1,i)=obstracle_R*cos(i*2*pi/50);
   Ly(1,i)=obstracle_R*sin(i*2*pi/50);
   Lz(2,i)=obstracle_hight;
   Lz(1,i)=0;
end
Lx(1,51)=Lx(1,1);
Ly(1,51)=Ly(1,1);
Lz(1,51)=0;
Lz(2,51)=obstracle_hight;
 
 
Lx(2,:)=Lx(1,:);
Ly(2,:)=Ly(1,:);
 
for i=1:51
    out=Trans*[Lx(1,i);Ly(1,i);Lz(1,i);1];
    Lx(1,i)=out(1);
    Ly(1,i)=out(2);
    Lz(1,i)=out(3);
    out=Trans*[Lx(2,i);Ly(2,i);Lz(2,i);1];
    Lx(2,i)=out(1);
    Ly(2,i)=out(2);
    Lz(2,i)=out(3);    
end
hold on; grid on;
axis equal;
axis([0 160 -10 160 -10 120]);
view(-50,20);
surf(Lx,Ly,Lz,'FaceColor',[1,0,0]);
fill3(Lx(1,:),Ly(1,:),Lz(1,:),[1,0,0]);
fill3(Lx(2,:),Ly(2,:),Lz(2,:),[1,0,0]);
xlabel('X');
ylabel('Y');
zlabel('Z');