Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Kalman Filter test model #3

Merged
merged 2 commits into from
Apr 15, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions kalman_filter/KalmanFilter.prj
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version="1.0" encoding="UTF-8"?>
<MATLABProject xmlns="http://www.mathworks.com/MATLABProjectFile" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" version="1.0"/>
2 changes: 0 additions & 2 deletions kalman_filter/README.md

This file was deleted.

Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Value="true" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="environment.loadSimulink" type="Command" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="" Type="Relative" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="90c946d4-8e41-4b3f-8efc-528cdd5956c1" type="Reference" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="tests" Type="Relative" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="21b403aa-f001-4393-9bd9-c12b5e201d40" type="Reference" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Type="StartUp" Visible="0" Icon="" File="kalman_filter.slx" Name="kalman_filter" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="edcb753c-3e96-4080-be2c-7ce3ec3ed6b8" type="EntryPoint" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Test" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="test" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Other" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="other" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Convenience" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="convenience" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="None" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="none" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Derived" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="derived" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Design" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="design" type="Label" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="READ_ONLY" Name="Artifact" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="artifact" type="Label" />
2 changes: 2 additions & 0 deletions kalman_filter/resources/project/Project.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info MetadataType="fixedPathV2" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="codegen" Type="Relative" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="SimulinkCodeGenFolder" type="Reference" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="cache" Type="Relative" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="SimulinkCacheFolder" type="Reference" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info ReadOnly="1" SingleValued="1" DataType="None" Name="Classification" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="FileClassCategory" type="Category" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="1" type="DIR_SIGNIFIER" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="test" />
</Category>
</Info>
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="robot_logger_device.mat" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="test" />
</Category>
</Info>
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="kalman_filter_test.slx" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="test" />
</Category>
</Info>
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="load_params.m" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design" />
</Category>
</Info>
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="kalman_filter.slx" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info>
<Category UUID="FileClassCategory">
<Label UUID="design" />
</Category>
</Info>
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="init.m" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="tests" type="File" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="Extensions" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="EnvironmentCustomizations" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="ProjectPath" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Name="KalmanFilter" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="ProjectData" type="Info" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="EntryPoints" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="ProjectData" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="WorkingFolders" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="Categories" />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info location="Root" type="Files" />
2 changes: 2 additions & 0 deletions kalman_filter/resources/project/rootp.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info />
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info />
Binary file added kalman_filter/tests/kalman_filter_test.slx
Binary file not shown.
33 changes: 33 additions & 0 deletions kalman_filter/tests/load_params.m
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
% Copyright (C) 2022 Fondazione Istitito Italiano di Tecnologia (IIT)
% All Rights Reserved.
%
% Author: Simone Girardi <[email protected]>

%load('robot_logger_device.mat');
load('robot_logged_data.mat');
Ts = 0.001;

%N = 25;
%p = robot_logger_device.joints_state.positions.data(N, :)';
%v = robot_logger_device.joints_state.velocities.data(N, :)';
%a = robot_logger_device.joints_state.accelerations.data(N, :)';

p = robot_logged_data.p;
v = robot_logged_data.v;
a = robot_logged_data.a;

L = min([length(p), length(v), length(a)]);
%p = p(1:L);
%v = v(1:L);
%a = a(1:L);
t = (0:L-1)' * Ts;

simin = [t p v a];

order = 3;
assert(order>=3);

x0 = [simin(1, 2:end)'; zeros(order-3, 1)];
Q = [1e-4; 1e-2; 10];
R = 1e-4;
P0 = 9.9e-05;
3 changes: 3 additions & 0 deletions kalman_filter/tests/robot_logged_data.mat
Git LFS file not shown