def test_cov(self): config, robot_params = loadSystem() block = ChainSensor( config["chainA"], ChainMeasurement(chain_id="chainA", chain_state=JointState(position=[0])), "boardA") block.update_config(robot_params) cov = block.compute_cov(None) print cov
def test_cov(self): config, robot_params = loadSystem() block = ChainSensor(config["chainA"], ChainMeasurement(chain_id="chainA", chain_state=JointState(position=[0]) ), "boardA") block.update_config(robot_params) cov = block.compute_cov(None) print cov
def test_cov(self): config, robot_params = loadSystem() block = ChainSensor(config["chainA"], ChainMeasurement(chain_id="chainA", chain_state=JointState(position=[0]) ), "boardA") block.update_config(robot_params) cov = block.compute_cov(None) print cov self.assertAlmostEqual(cov[0,0], 0.0, 6) self.assertAlmostEqual(cov[1,0], 0.0, 6) self.assertAlmostEqual(cov[1,1], 1.0, 6) self.assertAlmostEqual(cov[4,4], 4.0, 6)